Changeset 59af2cc in rtems


Ignore:
Timestamp:
03/07/17 12:59:09 (7 years ago)
Author:
Javier Jalle <javier.jalle@…>
Branches:
5, master
Children:
cc40f0b
Parents:
631f711
git-author:
Javier Jalle <javier.jalle@…> (03/07/17 12:59:09)
git-committer:
Daniel Hellstrom <daniel@…> (11/14/17 09:27:20)
Message:

leon, grspw_router: Changed driver C API

SMP safe by spin-lock protection and semaphore. On spin-lock per SpW/AMBA
port to allow multiple parallel register operations per port. A common
semaphore for general SpW router configuration such as routing table.

Move to a C API instead of using the I/O Manager. The SpW router driver
does not perform any I/O only management of the router.

Update #2355.

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

Legend:

Unmodified
Added
Removed
  • c/src/lib/libbsp/sparc/shared/include/grspw_router.h

    r631f711 r59af2cc  
    22 * GRSPW ROUTER APB-Register Driver.
    33 *
    4  * COPYRIGHT (c) 2010.
     4 * COPYRIGHT (c) 2010-2017.
    55 * Cobham Gaisler AB.
    66 *
     
    1313#define __GRSPW_ROUTER_H__
    1414
     15#ifdef __cplusplus
     16extern "C" {
     17#endif
     18
     19/* Maximum number of ROUTER devices supported by driver */
     20#define ROUTER_MAX 2
     21
     22#define ROUTER_ERR_OK 0
     23#define ROUTER_ERR_EINVAL -1
     24#define ROUTER_ERR_ERROR -2
     25#define ROUTER_ERR_TOOMANY -3
     26#define ROUTER_ERR_IMPLEMENTED -4
     27
    1528/* Hardware Information */
    1629struct router_hw_info {
    17         unsigned char nports_spw;
    18         unsigned char nports_amba;
    19         unsigned char nports_fifo;
    20         char timers_avail;
    21         char pnp_avail;
    22         unsigned char ver_major;
    23         unsigned char ver_minor;
    24         unsigned char ver_patch;
    25         unsigned char iid;
     30        uint8_t nports_spw;
     31        uint8_t nports_amba;
     32        uint8_t nports_fifo;
     33        int8_t srouting;
     34        int8_t pnp_enable;
     35        int8_t timers_avail;
     36        int8_t pnp_avail;
     37        uint8_t ver_major;
     38        uint8_t ver_minor;
     39        uint8_t ver_patch;
     40        uint8_t iid;
     41
     42        /* Router capabilities */
     43        uint8_t amba_port_fifo_size;
     44        uint8_t spw_port_fifo_size;
     45        uint8_t rmap_maxdlen;
     46        int8_t aux_async;
     47        int8_t aux_dist_int_support;
     48        int8_t dual_port_support;
     49        int8_t dist_int_support;
     50        int8_t spwd_support;
     51        uint8_t pktcnt_support;
     52        uint8_t charcnt_support;
    2653};
    2754
     
    3461
    3562struct router_config {
    36         unsigned int flags; /* Determine what configuration should be updated */
     63        uint32_t flags; /* Determine what configuration should be updated */
    3764
    3865        /* Router Configuration Register */
    39         unsigned int config;
     66        uint32_t config;
    4067
    4168        /* Set Instance ID */
    42         unsigned char iid;
     69        uint8_t iid;
    4370
    4471        /* SpaceWire Link Initialization Clock Divisor */
    45         unsigned char idiv;
    46 
    47         /* Timer Prescaler and Reload */
    48         unsigned int timer_prescaler;
    49         unsigned int timer_reload[32];
    50 };
    51 
    52 /* Logical routing table */
    53 struct router_routes {
    54         unsigned int route[224];
    55 };
    56 
    57 /* Port Setup, see register definitions for "Port setup register" */
    58 struct router_ps {
    59         unsigned int ps[31]; /* Port Setup for ports 1-31 */
    60         unsigned int ps_logical[224]; /* Port setup for locgical addresses 32-255 */
     72        uint8_t idiv;
     73
     74        /* Timer Prescaler */
     75        uint32_t timer_prescaler;
     76};
     77
     78/* Routing table address control */
     79struct router_route_acontrol {
     80        uint32_t control[31];
     81        uint32_t control_logical[224];
     82};
     83
     84/* Routing table port mapping */
     85struct router_route_portmap {
     86        uint32_t pmap[31]; /* Port Setup for ports 1-31 */
     87        uint32_t pmap_logical[224]; /* Port setup for locgical addresses 32-255 */
     88};
     89
     90/* Routing table */
     91#define ROUTER_ROUTE_FLG_MAP 0x01
     92#define ROUTER_ROUTE_FLG_CTRL 0x02
     93#define ROUTER_ROUTE_FLG_ALL 0x3        /* All Above Flags */
     94struct router_routing_table {
     95        uint32_t flags; /* Determine what configuration should be updated */
     96
     97        struct router_route_acontrol acontrol;
     98        struct router_route_portmap portmap;
    6199};
    62100
    63101/* Set/Get Port Control/Status */
    64 #define ROUTER_PORTFLG_SET_CTRL 0x01
    65 #define ROUTER_PORTFLG_GET_CTRL 0x02
    66 #define ROUTER_PORTFLG_SET_STS  0x04
    67 #define ROUTER_PORTFLG_GET_STS  0x08
     102#define ROUTER_PORT_FLG_SET_CTRL        0x01
     103#define ROUTER_PORT_FLG_GET_CTRL        0x02
     104#define ROUTER_PORT_FLG_SET_STS 0x04
     105#define ROUTER_PORT_FLG_GET_STS 0x08
     106#define ROUTER_PORT_FLG_SET_CTRL2       0x10
     107#define ROUTER_PORT_FLG_GET_CTRL2       0x20
     108#define ROUTER_PORT_FLG_SET_TIMER       0x40
     109#define ROUTER_PORT_FLG_GET_TIMER       0x80
     110#define ROUTER_PORT_FLG_SET_PKTLEN      0x100
     111#define ROUTER_PORT_FLG_GET_PKTLEN      0x200
    68112struct router_port {
    69         unsigned int flag;
    70         int port;
    71         unsigned int ctrl;
    72         unsigned int sts;
    73 };
    74 
    75 /* Get Hardware support/information available */
    76 #define GRSPWR_IOCTL_HWINFO     0x01    /* OUT:    struct router_hw_info */
    77 
    78 /* Router Configuration */
    79 #define GRSPWR_IOCTL_CFG_SET    0x02    /* IN:     struct router_config */
    80 #define GRSPWR_IOCTL_CFG_GET    0x03    /* OUT:    struct router_config */
    81 
    82 /* Routes */
    83 #define GRSPWR_IOCTL_ROUTES_SET 0x04    /* IN:     struct router_routes */
    84 #define GRSPWR_IOCTL_ROUTES_GET 0x05    /* OUT:    struct router_routes */
    85 
    86 /* Port Setup */
    87 #define GRSPWR_IOCTL_PS_SET     0x06    /* IN:     struct router_ps */
    88 #define GRSPWR_IOCTL_PS_GET     0x07    /* OUT:    struct router_ps */
    89 
    90 /* Set configuration write enable */
    91 #define GRSPWR_IOCTL_WE_SET     0x08    /* INT:    int */
    92 
    93 /* Set/Get Port Control/Status */
    94 #define GRSPWR_IOCTL_PORT       0x09    /* IN/OUT: struct router_port */
    95 
    96 /* Set Router Configuration/Status Register */
    97 #define GRSPWR_IOCTL_CFGSTS_SET 0x0a    /* IN:     unsigned int */
    98 /* Get Router Configuration/Status Register */
    99 #define GRSPWR_IOCTL_CFGSTS_GET 0x0b    /* OUT:    unsigned int */
    100 
    101 /* Get Current Time-Code Register */
    102 #define GRSPWR_IOCTL_TC_GET     0x0c    /* OUT:    unsigned int */
     113        uint32_t flag;
     114        /* Port control */
     115        uint32_t ctrl;
     116        /* Port status */
     117        uint32_t sts;
     118        /* Port control 2 */
     119        uint32_t ctrl2;
     120        /* Timer  Reload */
     121        uint32_t timer_reload;
     122        /* Maximum packet length */
     123        uint32_t packet_length;
     124};
    103125
    104126/* Register GRSPW Router driver to Driver Manager */
    105 void router_register_drv (void);
    106 
     127void router_register_drv(void);
     128
     129extern void *router_open(unsigned int dev_no);
     130extern int router_close(void *d);
     131extern int router_print(void *d);
     132extern int router_hwinfo_get(void *d, struct router_hw_info *hwinfo);
     133
     134/* Router general config */
     135extern int router_config_set(void *d, struct router_config *cfg);
     136extern int router_config_get(void *d, struct router_config *cfg);
     137
     138/* Routing table config */
     139extern int router_routing_table_set(void *d,
     140                struct router_routing_table *cfg);
     141extern int router_routing_table_get(void *d,
     142                struct router_routing_table *cfg);
     143
     144/* Router Set/Get Port configuration */
     145extern int router_port_ioc(void *d, int port, struct router_port *cfg);
     146
     147/* Read Port Control register */
     148extern int router_port_ctrl_get(void *d, int port, uint32_t *ctrl);
     149/* Read Port Control2 register */
     150extern int router_port_ctrl2_get(void *d, int port, uint32_t *ctrl2);
     151/* Write Port Control Register */
     152extern int router_port_ctrl_set(void *d, int port, uint32_t ctrl);
     153/* Write Port Control2 Register */
     154extern int router_port_ctrl2_set(void *d, int port, uint32_t ctrl2);
     155/* Set Timer Reload Value for a specific port */
     156extern int router_port_treload_set(void *d, int port, uint32_t reload);
     157/* Get Timer Reload Value for a specific port */
     158extern int router_port_treload_get(void *d, int port, uint32_t *reload);
     159/* Get Maximum packet length for a specific port */
     160extern int router_port_maxplen_get(void *d, int port, uint32_t *length);
     161/* Set Maximum packet length for a specific port */
     162extern int router_port_maxplen_set(void *d, int port, uint32_t length);
     163
     164/* Check Port Status register and clear errors if there are */
     165extern int router_port_status(void *d, int port, uint32_t *sts);
     166
     167#define ROUTER_LINK_STATUS_ERROR_RESET 0
     168#define ROUTER_LINK_STATUS_ERROR_WAIT 1
     169#define ROUTER_LINK_STATUS_READY 2
     170#define ROUTER_LINK_STATUS_STARTED 3
     171#define ROUTER_LINK_STATUS_CONNECTING 4
     172#define ROUTER_LINK_STATUS_RUN_STATE 5
     173/* Get Link status */
     174extern int router_port_link_status(void *d, int port);
     175/* Operate a Link */
     176extern int router_port_enable(void *d, int port);
     177extern int router_port_disable(void *d, int port);
     178extern int router_port_link_stop(void *d, int port);
     179extern int router_port_link_start(void *d, int port);
     180extern int router_port_link_receive_spill(void *d, int port);
     181extern int router_port_link_transmit_reset(void *d, int port);
     182
     183/* Get port credit counter register */
     184extern int router_port_cred_get(void *d, int port, uint32_t *cred);
     185
     186/*
     187 * ROUTER RTACTRL register fields
     188 */
     189#define RTACTRL_SR (0x1 << RTACTRL_SR_BIT)
     190#define RTACTRL_EN (0x1 << RTACTRL_EN_BIT)
     191#define RTACTRL_PR (0x1 << RTACTRL_PR_BIT)
     192#define RTACTRL_HD (0x1 << RTACTRL_HD_BIT)
     193
     194#define RTACTRL_SR_BIT 3
     195#define RTACTRL_EN_BIT 2
     196#define RTACTRL_PR_BIT 1
     197#define RTACTRL_HD_BIT 0
     198
     199/* Individual route modification */
     200#define ROUTER_ROUTE_PACKETDISTRIBUTION_ENABLE (0x1 << 16)
     201#define ROUTER_ROUTE_PACKETDISTRIBUTION_DISABLE (0x0 << 16)
     202#define ROUTER_ROUTE_SPILLIFNOTREADY_ENABLE RTACTRL_SR
     203#define ROUTER_ROUTE_SPILLIFNOTREADY_DISABLE 0
     204#define ROUTER_ROUTE_ENABLE RTACTRL_EN
     205#define ROUTER_ROUTE_DISABLE 0
     206#define ROUTER_ROUTE_PRIORITY_HIGH RTACTRL_PR
     207#define ROUTER_ROUTE_PRIORITY_LOW 0
     208#define ROUTER_ROUTE_HEADERDELETION_ENABLE RTACTRL_HD
     209#define ROUTER_ROUTE_HEADERDELETION_DISABLE 0
     210struct router_route {
     211        uint8_t from_address;
     212        uint8_t to_port[32];
     213        int count;
     214        int options;
     215};
     216extern int router_route_set(void *d, struct router_route *route);
     217extern int router_route_get(void *d, struct router_route *route);
     218
     219/* Router configuration port write enable */
     220extern int router_write_enable(void *d);
     221extern int router_write_disable(void *d);
     222
     223/* Router reset */
     224extern int router_reset(void *d);
     225
     226/* Set Instance ID */
     227extern int router_instance_set(void *d, uint8_t iid);
     228/* Get Instance ID */
     229extern int router_instance_get(void *d, uint8_t *iid);
     230
     231/* Set SpaceWire Link Initialization Clock Divisor */
     232extern int router_idiv_set(void *d, uint8_t idiv);
     233/* Get SpaceWire Link Initialization Clock Divisor */
     234extern int router_idiv_get(void *d, uint8_t *idiv);
     235
     236/* Set Timer Prescaler */
     237extern int router_tpresc_set(void *d, uint32_t prescaler);
     238/* Get Timer Prescaler */
     239extern int router_tpresc_get(void *d, uint32_t *prescaler);
     240
     241/* Set/get Router configuration */
     242extern int router_cfgsts_set(void *d, uint32_t cfgsts);
     243extern int router_cfgsts_get(void *d, uint32_t *cfgsts);
     244
     245/* Router timecode */
     246extern int router_tc_enable(void *d);
     247extern int router_tc_disable(void *d);
     248extern int router_tc_reset(void *d);
     249extern int router_tc_get(void *d);
     250
     251/* Router Interrupts */
     252/*
     253 * ROUTER IMASK register fields
     254 */
     255#define IMASK_PE (0x1 << IMASK_PE_BIT)
     256#define IMASK_SR (0x1 << IMASK_SR_BIT)
     257#define IMASK_RS (0x1 << IMASK_RS_BIT)
     258#define IMASK_TT (0x1 << IMASK_TT_BIT)
     259#define IMASK_PL (0x1 << IMASK_PL_BIT)
     260#define IMASK_TS (0x1 << IMASK_TS_BIT)
     261#define IMASK_AC (0x1 << IMASK_AC_BIT)
     262#define IMASK_RE (0x1 << IMASK_RE_BIT)
     263#define IMASK_IA (0x1 << IMASK_IA_BIT)
     264#define IMASK_LE (0x1 << IMASK_LE_BIT)
     265#define IMASK_ME (0x1 << IMASK_ME_BIT)
     266#define IMASK_ALL ( IMASK_PE | IMASK_SR | IMASK_RS | IMASK_TT \
     267                IMASK_PL | IMASK_TS | IMASK_AC | IMASK_RE | IMASK_IA \
     268                IMASK_LE | IMASK_ME)
     269
     270#define IMASK_PE_BIT 10
     271#define IMASK_SR_BIT 9
     272#define IMASK_RS_BIT 8
     273#define IMASK_TT_BIT 7
     274#define IMASK_PL_BIT 6
     275#define IMASK_TS_BIT 5
     276#define IMASK_AC_BIT 4
     277#define IMASK_RE_BIT 3
     278#define IMASK_IA_BIT 2
     279#define IMASK_LE_BIT 1
     280#define IMASK_ME_BIT 0
     281
     282#define ROUTER_INTERRUPT_ALL IMASK_ALL
     283#define ROUTER_INTERRUPT_SPWPNP_ERROR IMASK_PE
     284#define ROUTER_INTERRUPT_SPILLED IMASK_SR
     285#define ROUTER_INTERRUPT_RUNSTATE IMASK_RS
     286#define ROUTER_INTERRUPT_TC_TRUNCATION IMASK_TT
     287#define ROUTER_INTERRUPT_PACKET_TRUNCATION IMASK_PL
     288#define ROUTER_INTERRUPT_TIMEOUT IMASK_TS
     289#define ROUTER_INTERRUPT_CFGPORT IMASK_AC
     290#define ROUTER_INTERRUPT_RMAP_ERROR IMASK_RE
     291#define ROUTER_INTERRUPT_INVALID_ADDRESS IMASK_IA
     292#define ROUTER_INTERRUPT_LINK_ERROR IMASK_LE
     293#define ROUTER_INTERRUPT_MEMORY_ERROR IMASK_ME
     294extern int router_port_interrupt_unmask(void *d, int port);
     295extern int router_port_interrupt_mask(void *d, int port);
     296extern int router_interrupt_unmask(void *d, int options);
     297extern int router_interrupt_mask(void *d, int options);
     298
     299/* Router Interrupt code generation */
     300/*
     301 * ROUTER ICODEGEN register fields
     302 */
     303#define ICODEGEN_UA (0x1 << ICODEGEN_UA_BIT)
     304#define ICODEGEN_AH (0x1 << ICODEGEN_AH_BIT)
     305#define ICODEGEN_IT (0x1 << ICODEGEN_IT_BIT)
     306#define ICODEGEN_TE (0x1 << ICODEGEN_TE_BIT)
     307#define ICODEGEN_EN (0x1 << ICODEGEN_EN_BIT)
     308#define ICODEGEN_IN (0x1f << ICODEGEN_IN_BIT)
     309
     310#define ICODEGEN_UA_BIT 20
     311#define ICODEGEN_AH_BIT 19
     312#define ICODEGEN_IT_BIT 18
     313#define ICODEGEN_TE_BIT 17
     314#define ICODEGEN_EN_BIT 16
     315#define ICODEGEN_IN_BIT 0
     316
     317#define ROUTER_ICODEGEN_ITYPE_EDGE ICODEGEN_IT
     318#define ROUTER_ICODEGEN_ITYPE_LEVEL 0
     319#define ROUTER_ICODEGEN_AUTOUNACK_ENABLE ICODEGEN_UA
     320#define ROUTER_ICODEGEN_AUTOUNACK_DISABLE 0
     321#define ROUTER_ICODEGEN_AUTOACK_ENABLE ICODEGEN_AH
     322#define ROUTER_ICODEGEN_AUTOACK_DISABLE 0
     323extern int router_icodegen_enable(void *d, uint8_t intn, uint32_t aitimer,
     324                int options);
     325extern int router_icodegen_disable(void *d);
     326
     327/* Router interrupt change timers */
     328extern int router_isrctimer_set(void *d, uint32_t reloadvalue);
     329extern int router_isrctimer_get(void *d, uint32_t *reloadvalue);
     330
     331/* Router interrupt timers */
     332extern int router_isrtimer_set(void *d, uint32_t reloadvalue);
     333extern int router_isrtimer_get(void *d, uint32_t *reloadvalue);
     334
     335#ifdef __cplusplus
     336}
    107337#endif
     338
     339#endif
  • c/src/lib/libbsp/sparc/shared/spw/grspw_router.c

    r631f711 r59af2cc  
    11/* GRSPW ROUTER APB-Register Driver.
    22 *
    3  * COPYRIGHT (c) 2010.
     3 * COPYRIGHT (c) 2010-2017.
    44 * Cobham Gaisler AB.
    55 *
     
    1313#include <rtems/bspIo.h>
    1414#include <stdio.h>
     15#include <bsp.h>
     16#include <rtems/bspIo.h> /* printk */
    1517
    1618#include <drvmgr/drvmgr.h>
     
    1820#include <bsp/grspw_router.h>
    1921
    20 #define ROUTER_DBG(args...)
    21 
    22 #define REG_READ(adr) (*(volatile unsigned int *)(adr))
    23 #define REG_WRITE(adr, value) (*(volatile unsigned int *)(adr) = (value))
     22//#define STATIC
     23#define STATIC static
     24
     25#define UNUSED __attribute__((unused))
     26
     27//#define DEBUG 1
     28
     29#ifdef DEBUG
     30#define DBG(x...) printf(x)
     31#else
     32#define DBG(x...)
     33#endif
     34
     35#define THREAD_SAFE 1
     36
     37/* Use interrupt lock privmitives compatible with SMP defined in
     38 * RTEMS 4.11.99 and higher.
     39 */
     40#if (((__RTEMS_MAJOR__ << 16) | (__RTEMS_MINOR__ << 8) | __RTEMS_REVISION__) >= 0x040b63)
     41
     42#ifdef THREAD_SAFE
     43/* map via rtems_interrupt_lock_* API: */
     44#define SPIN_DECLARE(lock) RTEMS_INTERRUPT_LOCK_MEMBER(lock)
     45#define SPIN_INIT(lock, name) rtems_interrupt_lock_initialize(lock, name)
     46#define SPIN_LOCK(lock, level) rtems_interrupt_lock_acquire_isr(lock, &level)
     47#define SPIN_LOCK_IRQ(lock, level) rtems_interrupt_lock_acquire(lock, &level)
     48#define SPIN_UNLOCK(lock, level) rtems_interrupt_lock_release_isr(lock, &level)
     49#define SPIN_UNLOCK_IRQ(lock, level) rtems_interrupt_lock_release(lock, &level)
     50#define SPIN_IRQFLAGS(k) rtems_interrupt_lock_context k
     51#define SPIN_ISR_IRQFLAGS(k) SPIN_IRQFLAGS(k)
     52#define SPIN_FREE(lock) rtems_interrupt_lock_destroy(lock)
     53#else
     54#define SPIN_DECLARE(lock)
     55#define SPIN_INIT(lock, name)
     56#define SPIN_LOCK(lock, level)
     57#define SPIN_LOCK_IRQ(lock, level)
     58#define SPIN_UNLOCK(lock, level)
     59#define SPIN_UNLOCK_IRQ(lock, level)
     60#define SPIN_IRQFLAGS(k)
     61#define SPIN_ISR_IRQFLAGS(k)
     62#define SPIN_FREE(lock)
     63#endif
     64
     65#else
     66
     67#ifdef THREAD_SAFE
     68#error THREAD SAFE operation not supported on this RTEMS version
     69#else
     70#define SPIN_DECLARE(lock)
     71#define SPIN_INIT(lock, name)
     72#define SPIN_LOCK(lock, level)
     73#define SPIN_LOCK_IRQ(lock, level)
     74#define SPIN_UNLOCK(lock, level)
     75#define SPIN_UNLOCK_IRQ(lock, level)
     76#define SPIN_IRQFLAGS(k)
     77#define SPIN_ISR_IRQFLAGS(k)
     78#define SPIN_FREE(lock)
     79#endif
     80
     81#ifdef RTEMS_SMP
     82#error SMP mode not compatible with these interrupt lock primitives
     83#endif
     84
     85#endif
     86
     87#define REG_WRITE(addr, val) (*(volatile unsigned int *)(addr) = (unsigned int)(val))
     88#define REG_READ(addr) (*(volatile unsigned int *)(addr))
     89
     90
     91/*
     92 * ROUTER RTPMAP register fields
     93 */
     94#define RTPMAP_PE (0x7fffffff << RTPMAP_PE_BIT)
     95#define RTPMAP_PD (0x1 << RTPMAP_PD_BIT)
     96
     97#define RTPMAP_PE_BIT 1
     98#define RTPMAP_PD_BIT 0
     99
     100/*
     101 * ROUTER RTACTRL register fields
     102 * DEFINED IN HEADER
     103 */
     104
     105/*
     106 * ROUTER PCTRL register fields
     107 */
     108#define PCTRL_RD (0xff << PCTRL_RD_BIT)
     109#define PCTRL_ST (0x1 << PCTRL_ST_BIT)
     110#define PCTRL_SR (0x1 << PCTRL_SR_BIT)
     111#define PCTRL_AD (0x1 << PCTRL_AD_BIT)
     112#define PCTRL_LR (0x1 << PCTRL_LR_BIT)
     113#define PCTRL_PL (0x1 << PCTRL_PL_BIT)
     114#define PCTRL_TS (0x1 << PCTRL_TS_BIT)
     115#define PCTRL_IC (0x1 << PCTRL_IC_BIT)
     116#define PCTRL_ET (0x1 << PCTRL_ET_BIT)
     117#define PCTRL_NP (0x1 << PCTRL_NP_BIT)
     118#define PCTRL_PS (0x1 << PCTRL_PS_BIT)
     119#define PCTRL_BE (0x1 << PCTRL_BE_BIT)
     120#define PCTRL_DI (0x1 << PCTRL_DI_BIT)
     121#define PCTRL_TR (0x1 << PCTRL_TR_BIT)
     122#define PCTRL_PR (0x1 << PCTRL_PR_BIT)
     123#define PCTRL_TF (0x1 << PCTRL_TF_BIT)
     124#define PCTRL_RS (0x1 << PCTRL_RS_BIT)
     125#define PCTRL_TE (0x1 << PCTRL_TE_BIT)
     126#define PCTRL_CE (0x1 << PCTRL_CE_BIT)
     127#define PCTRL_AS (0x1 << PCTRL_AS_BIT)
     128#define PCTRL_LS (0x1 << PCTRL_LS_BIT)
     129#define PCTRL_LD (0x1 << PCTRL_LD_BIT)
     130
     131#define PCTRL_RD_BIT 24
     132#define PCTRL_ST_BIT 21
     133#define PCTRL_SR_BIT 20
     134#define PCTRL_AD_BIT 19
     135#define PCTRL_LR_BIT 18
     136#define PCTRL_PL_BIT 17
     137#define PCTRL_TS_BIT 16
     138#define PCTRL_IC_BIT 15
     139#define PCTRL_ET_BIT 14
     140#define PCTRL_NP_BIT 13
     141#define PCTRL_PS_BIT 12
     142#define PCTRL_BE_BIT 11
     143#define PCTRL_DI_BIT 10
     144#define PCTRL_TR_BIT 9
     145#define PCTRL_PR_BIT 8
     146#define PCTRL_TF_BIT 7
     147#define PCTRL_RS_BIT 6
     148#define PCTRL_TE_BIT 5
     149#define PCTRL_CE_BIT 3
     150#define PCTRL_AS_BIT 2
     151#define PCTRL_LS_BIT 1
     152#define PCTRL_LD_BIT 0
     153
     154/*
     155 * ROUTER PSTSCFG register fields
     156 */
     157#define PSTSCFG_EO (0x1 << PSTSCFG_EO_BIT)
     158#define PSTSCFG_EE (0x1 << PSTSCFG_EE_BIT)
     159#define PSTSCFG_PL (0x1 << PSTSCFG_PL_BIT)
     160#define PSTSCFG_TT (0x1 << PSTSCFG_TT_BIT)
     161#define PSTSCFG_PT (0x1 << PSTSCFG_PT_BIT)
     162#define PSTSCFG_HC (0x1 << PSTSCFG_HC_BIT)
     163#define PSTSCFG_PI (0x1 << PSTSCFG_PI_BIT)
     164#define PSTSCFG_CE (0x1 << PSTSCFG_CE_BIT)
     165#define PSTSCFG_EC (0xf << PSTSCFG_EC_BIT)
     166#define PSTSCFG_TS (0x1 << PSTSCFG_TS_BIT)
     167#define PSTSCFG_ME (0x1 << PSTSCFG_ME_BIT)
     168#define PSTSCFG_IP (0x1f << PSTSCFG_IP_BIT)
     169#define PSTSCFG_CP (0x1 << PSTSCFG_CP_BIT)
     170#define PSTSCFG_PC (0xf << PSTSCFG_PC_BIT)
     171#define PSTSCFG_WCLEAR (PSTSCFG_EO | PSTSCFG_EE | PSTSCFG_PL | \
     172                                                PSTSCFG_TT | PSTSCFG_PT | PSTSCFG_HC | \
     173                                                PSTSCFG_PI | PSTSCFG_CE | PSTSCFG_TS | \
     174                                                PSTSCFG_ME | PSTSCFG_CP)
     175
     176#define PSTSCFG_EO_BIT 31
     177#define PSTSCFG_EE_BIT 30
     178#define PSTSCFG_PL_BIT 29
     179#define PSTSCFG_TT_BIT 28
     180#define PSTSCFG_PT_BIT 27
     181#define PSTSCFG_HC_BIT 26
     182#define PSTSCFG_PI_BIT 25
     183#define PSTSCFG_CE_BIT 24
     184#define PSTSCFG_EC_BIT 20
     185#define PSTSCFG_TS_BIT 18
     186#define PSTSCFG_ME_BIT 17
     187#define PSTSCFG_IP_BIT 7
     188#define PSTSCFG_CP_BIT 4
     189#define PSTSCFG_PC_BIT 0
     190
     191/*
     192 * ROUTER PSTS register fields
     193 */
     194#define PSTS_PT (0x3 << PSTS_PT_BIT)
     195#define PSTS_PL (0x1 << PSTS_PL_BIT)
     196#define PSTS_TT (0x1 << PSTS_TT_BIT)
     197#define PSTS_RS (0x1 << PSTS_RS_BIT)
     198#define PSTS_SR (0x1 << PSTS_SR_BIT)
     199#define PSTS_LR (0x1 << PSTS_LR_BIT)
     200#define PSTS_SP (0x1 << PSTS_SP_BIT)
     201#define PSTS_AC (0x1 << PSTS_AC_BIT)
     202#define PSTS_TS (0x1 << PSTS_TS_BIT)
     203#define PSTS_ME (0x1 << PSTS_ME_BIT)
     204#define PSTS_TF (0x1 << PSTS_TF_BIT)
     205#define PSTS_RE (0x1 << PSTS_RE_BIT)
     206#define PSTS_LS (0x7 << PSTS_LS_BIT)
     207#define PSTS_IP (0x1f << PSTS_IP_BIT)
     208#define PSTS_PR (0x1 << PSTS_PR_BIT)
     209#define PSTS_PB (0x1 << PSTS_PB_BIT)
     210#define PSTS_IA (0x1 << PSTS_IA_BIT)
     211#define PSTS_CE (0x1 << PSTS_CE_BIT)
     212#define PSTS_ER (0x1 << PSTS_ER_BIT)
     213#define PSTS_DE (0x1 << PSTS_DE_BIT)
     214#define PSTS_PE (0x1 << PSTS_PE_BIT)
     215#define PSTS_WCLEAR (PSTS_PL | PSTS_TT | PSTS_RS | PSTS_SR | \
     216                                         PSTS_TS | PSTS_ME | PSTS_IA | PSTS_CE | \
     217                                         PSTS_ER | PSTS_DE | PSTS_PE)
     218
     219#define PSTS_PT_BIT 30
     220#define PSTS_PL_BIT 29
     221#define PSTS_TT_BIT 28
     222#define PSTS_RS_BIT 27
     223#define PSTS_SR_BIT 26
     224#define PSTS_LR_BIT 22
     225#define PSTS_SP_BIT 21
     226#define PSTS_AC_BIT 20
     227#define PSTS_TS_BIT 18
     228#define PSTS_ME_BIT 17
     229#define PSTS_TF_BIT 16
     230#define PSTS_RE_BIT 15
     231#define PSTS_LS_BIT 12
     232#define PSTS_IP_BIT 7
     233#define PSTS_PR_BIT 6
     234#define PSTS_PB_BIT 5
     235#define PSTS_IA_BIT 4
     236#define PSTS_CE_BIT 3
     237#define PSTS_ER_BIT 2
     238#define PSTS_DE_BIT 1
     239#define PSTS_PE_BIT 0
     240
     241/*
     242 * ROUTER PTIMER register fields
     243 */
     244#define PTIMER_RL (0xffff << PTIMER_RL_BIT)
     245
     246#define PTIMER_RL_BIT 0
     247
     248/*
     249 * ROUTER PCTRL2 register fields
     250 */
     251#define PCTRL2_SM (0xff << PCTRL2_SM_BIT)
     252#define PCTRL2_SV (0xff << PCTRL2_SV_BIT)
     253#define PCTRL2_OR (0x1 << PCTRL2_OR_BIT)
     254#define PCTRL2_UR (0x1 << PCTRL2_UR_BIT)
     255#define PCTRL2_AT (0x1 << PCTRL2_AT_BIT)
     256#define PCTRL2_AR (0x1 << PCTRL2_AR_BIT)
     257#define PCTRL2_IT (0x1 << PCTRL2_IT_BIT)
     258#define PCTRL2_IR (0x1 << PCTRL2_IR_BIT)
     259#define PCTRL2_SD (0x1f << PCTRL2_SD_BIT)
     260#define PCTRL2_SC (0x1f << PCTRL2_SC_BIT)
     261
     262#define PCTRL2_SM_BIT 24
     263#define PCTRL2_SV_BIT 16
     264#define PCTRL2_OR_BIT 15
     265#define PCTRL2_UR_BIT 14
     266#define PCTRL2_AT_BIT 12
     267#define PCTRL2_AR_BIT 11
     268#define PCTRL2_IT_BIT 10
     269#define PCTRL2_IR_BIT 9
     270#define PCTRL2_SD_BIT 1
     271#define PCTRL2_SC_BIT 0
     272
     273/*
     274 * ROUTER RTRCFG register fields
     275 */
     276#define RTRCFG_SP (0x1f << RTRCFG_SP_BIT)
     277#define RTRCFG_AP (0x1f << RTRCFG_AP_BIT)
     278#define RTRCFG_FP (0x1f << RTRCFG_FP_BIT)
     279#define RTRCFG_SR (0x1 << RTRCFG_SR_BIT)
     280#define RTRCFG_PE (0x1 << RTRCFG_PE_BIT)
     281#define RTRCFG_IC (0x1 << RTRCFG_IC_BIT)
     282#define RTRCFG_IS (0x1 << RTRCFG_IS_BIT)
     283#define RTRCFG_IP (0x1 << RTRCFG_IP_BIT)
     284#define RTRCFG_AI (0x1 << RTRCFG_AI_BIT)
     285#define RTRCFG_AT (0x1 << RTRCFG_AT_BIT)
     286#define RTRCFG_IE (0x1 << RTRCFG_IE_BIT)
     287#define RTRCFG_RE (0x1 << RTRCFG_RE_BIT)
     288#define RTRCFG_EE (0x1 << RTRCFG_EE_BIT)
     289#define RTRCFG_LS (0x1 << RTRCFG_LS_BIT)
     290#define RTRCFG_SA (0x1 << RTRCFG_SA_BIT)
     291#define RTRCFG_TF (0x1 << RTRCFG_TF_BIT)
     292#define RTRCFG_ME (0x1 << RTRCFG_ME_BIT)
     293#define RTRCFG_TA (0x1 << RTRCFG_TA_BIT)
     294#define RTRCFG_PP (0x1 << RTRCFG_PP_BIT)
     295#define RTRCFG_WCLEAR (RTRCFG_ME)
     296
     297#define RTRCFG_SP_BIT 27
     298#define RTRCFG_AP_BIT 22
     299#define RTRCFG_FP_BIT 17
     300#define RTRCFG_SR_BIT 15
     301#define RTRCFG_PE_BIT 14
     302#define RTRCFG_IC_BIT 13
     303#define RTRCFG_IS_BIT 12
     304#define RTRCFG_IP_BIT 11
     305#define RTRCFG_AI_BIT 10
     306#define RTRCFG_AT_BIT 9
     307#define RTRCFG_IE_BIT 8
     308#define RTRCFG_RE_BIT 7
     309#define RTRCFG_EE_BIT 6
     310#define RTRCFG_LS_BIT 5
     311#define RTRCFG_SA_BIT 4
     312#define RTRCFG_TF_BIT 3
     313#define RTRCFG_ME_BIT 2
     314#define RTRCFG_TA_BIT 1
     315#define RTRCFG_PP_BIT 0
     316
     317/*
     318 * ROUTER TC register fields
     319 */
     320#define TC_RE (0x3f << TC_RE_BIT)
     321#define TC_EN (0x3f << TC_EN_BIT)
     322#define TC_CF (0x3f << TC_CF_BIT)
     323#define TC_TC (0x3f << TC_TC_BIT)
     324
     325#define TC_RE_BIT 9
     326#define TC_EN_BIT 8
     327#define TC_CF_BIT 6
     328#define TC_TC_BIT 0
     329
     330/*
     331 * ROUTER VER register fields
     332 */
     333#define VER_MA (0xff << VER_MA_BIT)
     334#define VER_MI (0xff << VER_MI_BIT)
     335#define VER_PA (0xff << VER_PA_BIT)
     336#define VER_ID (0xff << VER_ID_BIT)
     337
     338#define VER_MA_BIT 24
     339#define VER_MI_BIT 16
     340#define VER_PA_BIT 8
     341#define VER_ID_BIT 0
     342
     343/*
     344 * ROUTER IDIV register fields
     345 */
     346#define IDIV_ID (0xff << IDIV_ID_BIT)
     347
     348#define IDIV_ID_BIT 0
     349
     350/*
     351 * ROUTER CFGWE register fields
     352 */
     353#define CFGWE_WE (0x1 << CFGWE_WE_BIT)
     354
     355#define CFGWE_WE_BIT 0
     356
     357/*
     358 * ROUTER PRESCALER register fields
     359 */
     360#define PRESCALER_RL (0xffff << PRESCALER_RL_BIT)
     361
     362#define PRESCALER_RL_BIT 0
     363
     364/*
     365 * ROUTER IMASK register fields
     366 * DEFINED IN HEADER
     367 */
     368
     369/*
     370 * ROUTER ICODEGEN register fields
     371 * DEFINED IN HEADER
     372 */
     373
     374/*
     375 * ROUTER ISRTIMER register fields
     376 */
     377#define ISRTIMER_RL (0xffff << ISRTIMER_RL_BIT)
     378
     379#define ISRTIMER_RL_BIT 0
     380
     381/*
     382 * ROUTER AITIMER register fields
     383 */
     384#define AITIMER_RL (0xffff << AITIMER_RL_BIT)
     385
     386#define AITIMER_RL_BIT 0
     387
     388/*
     389 * ROUTER ISRCTIMER register fields
     390 */
     391#define ISRCTIMER_RL (0x1f << ISRCTIMER_RL_BIT)
     392
     393#define ISRCTIMER_RL_BIT 0
     394
     395/*
     396 * ROUTER CAP register fields
     397 */
     398#define CAP_AF (0x3 << CAP_AF_BIT)
     399#define CAP_PF (0x7 << CAP_PF_BIT)
     400#define CAP_RM (0x7 << CAP_RM_BIT)
     401#define CAP_AS (0x1 << CAP_AS_BIT)
     402#define CAP_AX (0x1 << CAP_AX_BIT)
     403#define CAP_DP (0x1 << CAP_DP_BIT)
     404#define CAP_ID (0x1 << CAP_ID_BIT)
     405#define CAP_SD (0x1 << CAP_SD_BIT)
     406#define CAP_PC (0x1f << CAP_PC_BIT)
     407#define CAP_CC (0x1f << CAP_CC_BIT)
     408
     409#define CAP_AF_BIT 24
     410#define CAP_PF_BIT 29
     411#define CAP_RM_BIT 16
     412#define CAP_AS_BIT 14
     413#define CAP_AX_BIT 13
     414#define CAP_DP_BIT 12
     415#define CAP_ID_BIT 11
     416#define CAP_SD_BIT 10
     417#define CAP_PC_BIT 4
     418#define CAP_CC_BIT 0
     419
     420/*
     421 * ROUTER PNPVEND register fields
     422 */
     423#define PNPVEND_VI (0xffff << PNPVEND_VI_BIT)
     424#define PNPVEND_PI (0xffff << PNPVEND_PI_BIT)
     425
     426#define PNPVEND_VI_BIT 16
     427#define PNPVEND_PI_BIT 0
     428
     429/*
     430 * ROUTER PNPUVEND register fields
     431 */
     432#define PNPUVEND_VI (0xffff << PNPUVEND_VI_BIT)
     433#define PNPUVEND_PI (0xffff << PNPUVEND_PI_BIT)
     434
     435#define PNPUVEND_VI_BIT 16
     436#define PNPUVEND_PI_BIT 0
     437
     438/*
     439 * ROUTER MAXPLEN register fields
     440 */
     441#define MAXPLEN_ML (0xffffff << MAXPLEN_ML_BIT)
     442
     443#define MAXPLEN_ML_BIT 0
     444
     445/*
     446 * ROUTER CHARO register fields
     447 */
     448#define CHARO_OR (0x1 << CHARO_OR_BIT)
     449#define CHARO_CC (0x7fffffff << CHARO_CC_BIT)
     450
     451#define CHARO_OR_BIT 31
     452#define CHARO_CC_BIT 0
     453
     454/*
     455 * ROUTER CHARI register fields
     456 */
     457#define CHARI_OR (0x1 << CHARI_OR_BIT)
     458#define CHARI_CC (0x7fffffff << CHARI_CC_BIT)
     459
     460#define CHARI_OR_BIT 31
     461#define CHARI_CC_BIT 0
     462
     463/*
     464 * ROUTER PKTO register fields
     465 */
     466#define PKTO_OR (0x1 << PKTO_OR_BIT)
     467#define PKTO_CC (0x7fffffff << PKTO_CC_BIT)
     468
     469#define PKTO_OR_BIT 31
     470#define PKTO_CC_BIT 0
     471
     472/*
     473 * ROUTER PKTI register fields
     474 */
     475#define PKTI_OR (0x1 << PKTI_OR_BIT)
     476#define PKTI_CC (0x7fffffff << PKTI_CC_BIT)
     477
     478#define PKTI_OR_BIT 31
     479#define PKTI_CC_BIT 0
     480
     481/*
     482 * ROUTER CRED register fields
     483 */
     484#define CRED_OC (0x3f << CRED_OC_BIT)
     485#define CRED_IC (0x3f << CRED_IC_BIT)
     486
     487#define CRED_OC_BIT 6
     488#define CRED_IC_BIT 0
     489
     490/*
     491 * ROUTER RTRCOMB register fields
     492 */
     493#define RTRCOMB_SR (0x1 << RTRCOMB_SR_BIT)
     494#define RTRCOMB_EN (0x1 << RTRCOMB_EN_BIT)
     495#define RTRCOMB_PR (0x1 << RTRCOMB_PR_BIT)
     496#define RTRCOMB_HD (0x1 << RTRCOMB_HD_BIT)
     497#define RTRCOMB_PE (0x7ffff << RTRCOMB_PE_BIT)
     498#define RTRCOMB_PD (0x1 << RTRCOMB_PD_BIT)
     499
     500#define RTRCOMB_SR_BIT 31
     501#define RTRCOMB_EN_BIT 30
     502#define RTRCOMB_PR_BIT 29
     503#define RTRCOMB_HD_BIT 28
     504#define RTRCOMB_PE_BIT 1
     505#define RTRCOMB_PD_BIT 0
    24506
    25507struct router_regs {
    26508        unsigned int resv1;             /* 0x000 */
    27509        unsigned int psetup[255];       /* 0x004 */
    28         unsigned int resv2[32];         /* 0x400 */
    29         unsigned int routes[224];       /* 0x480 */
     510        unsigned int resv2;             /* 0x400 */
     511        unsigned int routes[255];       /* 0x404 */
    30512        unsigned int pctrl[32];         /* 0x800 */
    31513        unsigned int psts[32];          /* 0x880 */
    32514        unsigned int treload[32];       /* 0x900 */
    33         unsigned int resv3[32];         /* 0x980 */
     515        unsigned int pctrl2[32];        /* 0x980 */
    34516        unsigned int cfgsts;            /* 0xA00 */
    35517        unsigned int timecode;          /* 0xA04 */
     
    38520        unsigned int cfgwe;             /* 0xA10 */
    39521        unsigned int tprescaler;        /* 0xA14 */
    40         unsigned int resv4[123];        /* 0xA18 */
    41         unsigned int charo[31];         /* 0xC04 */
    42         unsigned int resv5;             /* 0xC80 */
     522        unsigned int imask;             /* 0xA18 */
     523        unsigned int ipmask;            /* 0xA1C */
     524        unsigned int pip;               /* 0xA20 */
     525        unsigned int icodegen;          /* 0xA24 */
     526        unsigned int isr0;              /* 0xA28 */
     527        unsigned int isr1;              /* 0xA2C */
     528        unsigned int isrtimer;          /* 0xA30 */
     529        unsigned int aitimer;           /* 0xA34 */
     530        unsigned int isrctimer;         /* 0xA38 */
     531        unsigned int resv4;             /* 0xA3C */
     532        unsigned int lrunsts;           /* 0xA40 */
     533        unsigned int cap;               /* 0xA44 */
     534        unsigned int resv5[111];        /* 0xA48 */
     535        unsigned int charo[31];         /* 0xC04 */ /* TODO check GR718 */
     536        unsigned int resv6;             /* 0xC80 */
    43537        unsigned int chari[31];         /* 0xC84 */
    44         unsigned int resv6;             /* 0xD00 */
     538        unsigned int resv7;             /* 0xD00 */
    45539        unsigned int pkto[31];          /* 0xD04 */
    46         unsigned int resv7;             /* 0xD80 */
     540        unsigned int resv8;             /* 0xD80 */
    47541        unsigned int pkti[31];          /* 0xD84 */
     542        unsigned int maxplen[32];               /* 0xE00 */
     543        unsigned int resv9;             /* 0xE80 */
     544        unsigned int credcnt[31];       /* 0xE84 */
     545        unsigned int resv10[64];        /* 0xF00 */
     546        unsigned int resv11;            /* 0x1000 */
     547        unsigned int rtcomb[255];       /* 0x1004 */
    48548};
    49549
    50550struct router_priv {
    51         char devName[32];
    52551        struct drvmgr_dev *dev;
     552
     553        /* ROUTER control registers */
    53554        struct router_regs *regs;
     555
     556        #ifdef THREAD_SAFE
     557        /* ROUTER semaphore */
     558        rtems_id sem;
     559        #endif
     560
     561        /* ROUTER driver register */
     562        char devname[9];
     563        int index;                      /* Index in order it was probed */
     564
    54565        int minor;
    55566        int open;
    56567        struct router_hw_info hwinfo;
    57568        int nports;
     569        int irq_init;
     570
     571        SPIN_DECLARE(plock[32])
     572
    58573};
    59574
    60 static rtems_device_driver router_initialize(
    61         rtems_device_major_number  major,
    62         rtems_device_minor_number  minor,
    63         void                    * arg
    64         );
    65 
    66 static rtems_device_driver router_open(
    67         rtems_device_major_number major,
    68         rtems_device_minor_number minor,
    69         void                    * arg
    70         );
    71 
    72 static rtems_device_driver router_close(
    73         rtems_device_major_number major,
    74         rtems_device_minor_number minor,
    75         void                    * arg
    76         );
    77 
    78 static rtems_device_driver router_control(
    79         rtems_device_major_number major,
    80         rtems_device_minor_number minor,
    81         void                    * arg
    82         );
    83 
    84 #define ROUTER_DRIVER_TABLE_ENTRY \
    85   { router_initialize, \
    86     router_open, \
    87     router_close, \
    88     NULL, \
    89     NULL, \
    90     router_control }
    91 
    92 static void router_hwinfo(
    93         struct router_priv *priv,
    94         struct router_hw_info *hwinfo);
    95 
    96 static rtems_driver_address_table router_driver = ROUTER_DRIVER_TABLE_ENTRY;
    97 static int router_driver_io_registered = 0;
    98 static rtems_device_major_number router_driver_io_major = 0;
    99 
    100 /******************* Driver manager interface ***********************/
     575int router_count = 0;
     576static struct router_priv *priv_tab[ROUTER_MAX];
    101577
    102578/* Driver prototypes */
    103 int router_register_io(rtems_device_major_number *m);
     579
     580STATIC int router_init(struct router_priv *priv);
     581STATIC void router_hwinfo(struct router_priv *priv,
     582                          struct router_hw_info *hwinfo);
     583STATIC int router_acontrol_set(struct router_priv *priv,
     584                struct router_route_acontrol *control);
     585STATIC int router_acontrol_get(struct router_priv *priv,
     586                struct router_route_acontrol *control);
     587STATIC int router_portmap_set(struct router_priv *priv,
     588                struct router_route_portmap *pmap);
     589STATIC int router_portmap_get(struct router_priv *priv,
     590                struct router_route_portmap *pmap);
     591
     592/* -IRQ handler */
     593void router_isr(void *arg);
    104594
    105595int router_init2(struct drvmgr_dev *dev);
    106596
    107 struct drvmgr_drv_ops router_ops = 
    108 {
    109         .init = {NULL,  router_init2, NULL, NULL},
     597struct drvmgr_drv_ops router_ops =
     598{
     599        .init = {NULL,  router_init2, NULL, NULL},
    110600        .remove = NULL,
    111601        .info = NULL
    112602};
    113603
    114 struct amba_dev_id router_ids[] = 
     604struct amba_dev_id router_ids[] =
    115605{
    116606        {VENDOR_GAISLER, GAISLER_SPW_ROUTER},
     
    137627void router_register_drv (void)
    138628{
     629        DBG("Registering SPW ROUTER driver\n");
    139630        drvmgr_drv_register(&router_drv_info.general);
     631}
     632
     633STATIC void router_hwinfo(struct router_priv *priv,
     634                struct router_hw_info *hwinfo)
     635{
     636        unsigned int tmp;
     637
     638        /* Find router info */
     639        tmp = REG_READ(&priv->regs->cfgsts);
     640        hwinfo->nports_spw   = (tmp & RTRCFG_SP) >> RTRCFG_SP_BIT;
     641        hwinfo->nports_amba  = (tmp & RTRCFG_AP) >> RTRCFG_AP_BIT;
     642        hwinfo->nports_fifo  = (tmp & RTRCFG_FP) >> RTRCFG_FP_BIT;
     643        hwinfo->srouting     = (tmp & RTRCFG_SR) >> RTRCFG_SR_BIT;
     644        hwinfo->pnp_enable   = (tmp & RTRCFG_PE) >> RTRCFG_PE_BIT;
     645        hwinfo->timers_avail = (tmp & RTRCFG_TA) >> RTRCFG_TA_BIT;
     646        hwinfo->pnp_avail    = (tmp & RTRCFG_PP) >> RTRCFG_PP_BIT;
     647
     648        tmp = REG_READ(&priv->regs->ver);
     649        hwinfo->ver_major = (tmp & VER_MA) >> VER_MA_BIT;
     650        hwinfo->ver_minor = (tmp & VER_MI) >> VER_MI_BIT;
     651        hwinfo->ver_patch = (tmp & VER_PA) >> VER_PA_BIT;
     652        hwinfo->iid       = (tmp & VER_ID) >> VER_ID_BIT;
     653
     654        /* Find router capabilities */
     655        tmp = REG_READ(&priv->regs->cap);
     656        hwinfo->amba_port_fifo_size = 4 << ((tmp & CAP_AF) >> CAP_AF_BIT);
     657        hwinfo->spw_port_fifo_size = 16 << ((tmp & CAP_PF) >> CAP_PF_BIT);
     658        hwinfo->rmap_maxdlen = 4 << ((tmp & CAP_RM) >> CAP_RM_BIT);
     659        hwinfo->aux_async = (tmp & CAP_AS) >> CAP_AS_BIT;
     660        hwinfo->aux_dist_int_support = (tmp & CAP_AX) >> CAP_AX_BIT;
     661        hwinfo->dual_port_support = (tmp & CAP_ID) >> CAP_ID_BIT;
     662        hwinfo->dist_int_support = (tmp & CAP_DP) >> CAP_DP_BIT;
     663        hwinfo->spwd_support = (tmp & CAP_SD) >> CAP_SD_BIT;
     664        hwinfo->pktcnt_support = (tmp & CAP_PC) >> CAP_PC_BIT;
     665        hwinfo->charcnt_support = (tmp & CAP_CC) >> CAP_CC_BIT;
     666}
     667
     668STATIC void router_hwinfo_print(struct router_hw_info *hwinfo)
     669{
     670        DBG(" -PORTS= SPW: %d, AMBA: %d, FIFO: %d\n", hwinfo->nports_spw,
     671                        hwinfo->nports_amba, hwinfo->nports_fifo);
     672        DBG(" -Static routing: %s, Timers: %s\n",
     673                        (hwinfo->srouting?"Enabled":"Disabled"),
     674                        (hwinfo->timers_avail?"Available":"N/A"));
     675        DBG(" -PnP: %s, %s\n",
     676                        (hwinfo->pnp_avail?"Available":"N/A"),
     677                        (hwinfo->pnp_enable?"Enabled":"Disabled"));
     678        DBG(" -Version= Major: 0x%02x, Minor: 0x%02x, Patch: 0x%02x, ID: 0x%02x\n",
     679                        hwinfo->ver_major, hwinfo->ver_minor,
     680                        hwinfo->ver_patch, hwinfo->iid);
     681        DBG(" -Aux: %s, AuxDistInt: %s, DistInt: %s, SPWD: %s, PKTCNT: %s, "
     682                "CHARCNT: %s\n",
     683                        (hwinfo->aux_async?"Async":"Sync"),
     684                        (hwinfo->aux_dist_int_support?"Supported":"N/A"),
     685                        (hwinfo->dist_int_support?"Supported":"N/A"),
     686                        (hwinfo->spwd_support?"Supported":"N/A"),
     687                        (hwinfo->pktcnt_support?"Supported":"N/A"),
     688                        (hwinfo->charcnt_support?"Supported":"N/A"));
     689}
     690
     691STATIC int router_acontrol_set(struct router_priv *priv,
     692                struct router_route_acontrol *control)
     693{
     694        int i;
     695        for (i=0; i<31; i++) {
     696                REG_WRITE(&priv->regs->routes[i], control->control[i]);
     697        }
     698        for (i=0; i<224; i++) {
     699                REG_WRITE(&priv->regs->routes[i+31], control->control_logical[i]);
     700        }
     701        return ROUTER_ERR_OK;
     702}
     703
     704STATIC int router_acontrol_get(struct router_priv *priv,
     705                struct router_route_acontrol *control)
     706{
     707        int i;
     708        for (i=0; i<31; i++) {
     709                control->control[i] = REG_READ(&priv->regs->routes[i]);
     710        }
     711        for (i=0; i<224; i++) {
     712                control->control_logical[i] = REG_READ(&priv->regs->routes[i+31]);
     713        }
     714        return ROUTER_ERR_OK;
     715}
     716
     717STATIC int router_portmap_set(struct router_priv *priv,
     718                struct router_route_portmap *pmap)
     719{
     720        int i;
     721        for (i=0; i<31; i++) {
     722                REG_WRITE(&priv->regs->psetup[i], pmap->pmap[i]);
     723        }
     724        for (i=0; i<224; i++) {
     725                REG_WRITE(&priv->regs->psetup[i+31], pmap->pmap_logical[i]);
     726        }
     727        return ROUTER_ERR_OK;
     728}
     729
     730STATIC int router_portmap_get(struct router_priv *priv,
     731                struct router_route_portmap *pmap)
     732{
     733        int i;
     734        for (i=0; i<31; i++) {
     735                pmap->pmap[i] = REG_READ(&priv->regs->psetup[i]);
     736        }
     737        for (i=0; i<224; i++) {
     738                pmap->pmap_logical[i] = REG_READ(&priv->regs->psetup[i+31]);
     739        }
     740        return ROUTER_ERR_OK;
     741}
     742
     743STATIC int router_init(struct router_priv *priv)
     744{
     745        #ifdef THREAD_SAFE
     746        int i;
     747
     748        /* Device Semaphore created with count = 1 */
     749        if (rtems_semaphore_create(
     750                        rtems_build_name('S', 'R', 'O', '0' + priv->index), 1,
     751                        RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | \
     752                        RTEMS_NO_INHERIT_PRIORITY | RTEMS_LOCAL | \
     753                        RTEMS_NO_PRIORITY_CEILING, 0, &priv->sem) != RTEMS_SUCCESSFUL) {
     754                return DRVMGR_FAIL;
     755        }
     756        #endif
     757
     758        /* Find router info */
     759        router_hwinfo(priv, &priv->hwinfo);
     760
     761        priv->open = 0;
     762        /* Number of ports has to consider the configuration port (1 + SPW + AMBA + FIFO) */
     763        priv->nports = 1 + priv->hwinfo.nports_spw + priv->hwinfo.nports_amba +
     764                priv->hwinfo.nports_fifo;
     765        if ((priv->nports < 2) || (priv->nports > 32)) {
     766                return DRVMGR_EIO;
     767        }
     768
     769        #ifdef THREAD_SAFE
     770        /* Init port spin-lock memory structures */
     771        for (i=0; i<priv->nports; i++) {
     772                SPIN_INIT(&priv->plock[i],"portlock");
     773        }
     774        #endif
     775
     776        /* DEBUG print */
     777        DBG("SPW ROUTER[%d] with following capabilities:\n", priv->index);
     778        router_hwinfo_print(&priv->hwinfo);
     779
     780        return DRVMGR_OK;
    140781}
    141782
     
    145786        struct amba_dev_info *ambadev;
    146787        struct ambapp_core *pnpinfo;
    147         char prefix[32];
    148         rtems_status_code status;
    149 
    150         if ( priv == NULL )
     788        unsigned int tmp;
     789        int i;
     790        int status;
     791
     792        DBG("SPW ROUTER[%d] on bus %s\n", dev->minor_drv, dev->parent->dev->name);
     793
     794        if (router_count >= ROUTER_MAX) {
     795                return DRVMGR_ENORES;
     796        }
     797
     798        if (priv == NULL) {
    151799                return DRVMGR_NOMEM;
     800        }
    152801        priv->dev = dev;
    153 
    154         /* Do initialization */
    155         if ( router_driver_io_registered == 0) {
    156                 /* Register the I/O driver only once for all cores */
    157                 if ( router_register_io(&router_driver_io_major) ) {
    158                         /* Failed to register I/O driver */
    159                         return DRVMGR_FAIL;
    160                 }
    161 
    162                 router_driver_io_registered = 1;
    163         }
    164802
    165803        /* Get device information from AMBA PnP information */
    166804        ambadev = (struct amba_dev_info *)priv->dev->businfo;
    167         if ( ambadev == NULL ) {
     805        if (ambadev == NULL) {
    168806                return DRVMGR_FAIL;
    169807        }
     
    172810        priv->minor = dev->minor_drv;
    173811
    174         /* Register character device in registered region */
    175         router_hwinfo(priv, &priv->hwinfo);
    176         priv->open = 0;
    177         priv->nports = priv->hwinfo.nports_spw + priv->hwinfo.nports_amba +
    178                         priv->hwinfo.nports_fifo;
    179         if ( (priv->nports < 2) || (priv->nports > 32) )
    180                 return DRVMGR_FAIL;
    181 
    182         /* Get Filesystem name prefix */
    183         prefix[0] = '\0';
    184         if ( drvmgr_get_dev_prefix(dev, prefix) ) {
    185                 /* Failed to get prefix, make sure of a unique FS name
    186                  * by using the driver minor.
    187                  */
    188                 sprintf(priv->devName, "/dev/router%d", dev->minor_drv);
     812        /* Initilize driver struct */
     813        status = router_init(priv);
     814        if (status != DRVMGR_OK) {
     815                return status;
     816        }
     817
     818        /* Startup Action:
     819         *  - Clear interrupts
     820         *  - Mask interrupts
     821         */
     822
     823        /* Mask interrupts in ROTUER */
     824        REG_WRITE(&priv->regs->imask,0);
     825        REG_WRITE(&priv->regs->ipmask,0);
     826
     827        /* Clear interrupts in ROTUER */
     828        REG_WRITE(&priv->regs->pip,0xffffffff);
     829
     830        /* Clear errors in router and ports */
     831        tmp = REG_READ(&priv->regs->cfgsts);
     832        REG_WRITE(&priv->regs->cfgsts, tmp | RTRCFG_WCLEAR);
     833        tmp = REG_READ(&priv->regs->psts[0]);
     834        REG_WRITE(&priv->regs->psts[0], tmp & PSTSCFG_WCLEAR);
     835        for (i=1; i<priv->nports; i++) {
     836                tmp = REG_READ(&priv->regs->psts[i]);
     837                REG_WRITE(&priv->regs->psts[i], tmp & PSTS_WCLEAR);
     838        }
     839
     840        /* Register driver internally */
     841        priv->index = router_count;
     842        priv_tab[priv->index] = priv;
     843        router_count++;
     844
     845        /* Device name */
     846        sprintf(priv->devname, "router%d", priv->index);
     847
     848        return DRVMGR_OK;
     849}
     850
     851void *router_open(unsigned int dev_no)
     852{
     853        struct router_priv *priv, *ret;
     854
     855        if (dev_no >= router_count) {
     856                DBG("ROUTER Wrong index %u\n", dev_no);
     857                return NULL;
     858        }
     859
     860        priv = priv_tab[dev_no];
     861
     862        if (priv == NULL) {
     863                DBG("ROUTER Device not initialized\n");
     864                return NULL;
     865        }
     866
     867        #ifdef THREAD_SAFE
     868        /* Take device lock - Wait until we get semaphore */
     869        if (rtems_semaphore_obtain(priv->sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
     870                        != RTEMS_SUCCESSFUL) {
     871                DBG("ROUTER Sempahore failed\n");
     872                return NULL;
     873        }
     874        #endif
     875
     876        if (priv->open) {
     877                DBG("ROUTER Device already opened\n");
     878                ret = NULL;
    189879        } else {
    190                 /* Got special prefix, this means we have a bus prefix
    191                  * And we should use our "bus minor"
    192                  */
    193                 sprintf(priv->devName, "/dev/%srouter%d", prefix, dev->minor_bus);
    194         }
    195 
    196         /* Register Device */
    197         status = rtems_io_register_name(priv->devName, router_driver_io_major, dev->minor_drv);
    198         if (status != RTEMS_SUCCESSFUL) {
    199                 return DRVMGR_FAIL;
    200         }
    201 
    202         return DRVMGR_OK;
    203 }
    204 
    205 int router_register_io(rtems_device_major_number *m)
    206 {
    207         rtems_status_code r;
    208 
    209         if ((r = rtems_io_register_driver(0, &router_driver, m)) == RTEMS_SUCCESSFUL) {
    210                 ROUTER_DBG("ROUTER driver successfully registered, major: %d\n", *m);
     880                /* Take the device */
     881                priv->open = 1;
     882                ret = priv;
     883        }
     884        #ifdef THREAD_SAFE
     885        /* Unlock dev */
     886        rtems_semaphore_release(priv->sem);
     887        #endif
     888
     889        return ret;
     890}
     891
     892int router_close(void *d)
     893{
     894        struct router_priv *priv = d;
     895        int ret;
     896
     897        if (priv == NULL) {
     898                DBG("ROUTER Device not initialized\n");
     899                return ROUTER_ERR_EINVAL;
     900        }
     901
     902        #ifdef THREAD_SAFE
     903        /* Take device lock - Wait until we get semaphore */
     904        if (rtems_semaphore_obtain(priv->sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
     905                        != RTEMS_SUCCESSFUL) {
     906                DBG("ROUTER Sempahore failed\n");
     907                return ROUTER_ERR_ERROR;
     908        }
     909        #endif
     910
     911        if (priv->open == 0) {
     912                DBG("ROUTER Device already closed\n");
     913                ret = ROUTER_ERR_ERROR;
    211914        } else {
    212                 switch(r) {
    213                 case RTEMS_TOO_MANY:
    214                         printk("ROUTER rtems_io_register_driver failed: RTEMS_TOO_MANY\n");
    215                         return -1;
    216                 case RTEMS_INVALID_NUMBER: 
    217                         printk("ROUTER rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n");
    218                         return -1;
    219                 case RTEMS_RESOURCE_IN_USE:
    220                         printk("ROUTER rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n");
    221                         return -1;
    222                 default:
    223                         printk("ROUTER rtems_io_register_driver failed\n");
    224                         return -1;
     915                /* Mark not open */
     916                priv->open = 0;
     917                ret = ROUTER_ERR_OK;
     918        }
     919
     920        #ifdef THREAD_SAFE
     921        /* Unlock dev */
     922        rtems_semaphore_release(priv->sem);
     923        #endif
     924
     925        return ret;
     926}
     927
     928STATIC int router_check_open(void *d)
     929{
     930        struct router_priv *priv = d;
     931
     932        if (priv == NULL) {
     933                DBG("ROUTER Device not initialized\n");
     934                return ROUTER_ERR_EINVAL;
     935        }
     936
     937        if (priv->open == 0) {
     938                DBG("ROUTER Device closed\n");
     939                return ROUTER_ERR_ERROR;
     940        }
     941
     942        return 0;
     943}
     944
     945STATIC int router_check_port(void *d, int port)
     946{
     947        int ret = router_check_open(d);
     948
     949        if (ret == 0) {
     950                struct router_priv *priv = d;
     951                if((port < 0) || (port >= priv->nports)) {
     952                        DBG("ROUTER wrong port\n");
     953                        ret = ROUTER_ERR_EINVAL;
    225954                }
    226955        }
    227         return 0;
    228 }
    229 
    230 static rtems_device_driver router_initialize(
    231         rtems_device_major_number major,
    232         rtems_device_minor_number minor,
    233         void *arg
    234         )
    235 {
    236         return RTEMS_SUCCESSFUL;
    237 }
    238 
    239 static rtems_device_driver router_open(
    240         rtems_device_major_number major,
    241         rtems_device_minor_number minor,
    242         void                    * arg
    243         )
    244 {
    245         struct router_priv *priv;
    246         struct drvmgr_dev *dev;
    247 
    248         if ( drvmgr_get_dev(&router_drv_info.general, minor, &dev) ) {
    249                 ROUTER_DBG("Wrong minor %d\n", minor);
    250                 return RTEMS_INVALID_NAME;
    251         }
    252         priv = (struct router_priv *)dev->priv;
    253 
    254         if ( !priv || priv->open ) {
    255                 return RTEMS_RESOURCE_IN_USE;
    256         }
    257 
    258         priv->open = 1;
    259 
    260         return RTEMS_SUCCESSFUL;
    261 }
    262 
    263 static rtems_device_driver router_close(
    264         rtems_device_major_number major,
    265         rtems_device_minor_number minor,
    266         void                    * arg
    267         )
    268 {
    269         struct router_priv *priv;
    270         struct drvmgr_dev *dev;
    271 
    272         if ( drvmgr_get_dev(&router_drv_info.general, minor, &dev) ) {
    273                 ROUTER_DBG("Wrong minor %d\n", minor);
    274                 return RTEMS_INVALID_NAME;
    275         }
    276         priv = (struct router_priv *)dev->priv;
    277 
    278         priv->open = 0;
    279 
    280         return RTEMS_SUCCESSFUL;
    281 }
    282 
    283 static void router_hwinfo(
    284         struct router_priv *priv,
    285         struct router_hw_info *hwinfo)
    286 {
    287         unsigned int tmp;
    288 
    289         tmp = REG_READ(&priv->regs->cfgsts);
    290         hwinfo->nports_spw   = (tmp >> 27) & 0x1f;
    291         hwinfo->nports_amba  = (tmp >> 22) & 0x1f;
    292         hwinfo->nports_fifo  = (tmp >> 17) & 0x1f;
    293         hwinfo->timers_avail = (tmp >>  1) & 0x1;
    294         hwinfo->pnp_avail    = (tmp >>  0) & 0x1;
    295 
    296         tmp = REG_READ(&priv->regs->ver);
    297         hwinfo->ver_major   = (tmp >> 24) & 0xff;
    298         hwinfo->ver_minor   = (tmp >> 16) & 0xff;
    299         hwinfo->ver_patch   = (tmp >>  8) & 0xff;
    300         hwinfo->iid         = (tmp >>  0) & 0xff;
    301 }
    302 
    303 static int router_config_set(
    304         struct router_priv *priv,
    305         struct router_config *cfg)
    306 {
     956
     957        return ret;
     958}
     959
     960STATIC int router_check_distint_support(void *d)
     961{
     962        int ret = router_check_open(d);
     963
     964        if (ret == 0) {
     965                struct router_priv *priv = d;
     966                if (priv->hwinfo.dist_int_support == 0) {
     967                        DBG("ROUTER Dist interrupts not supported\n");
     968                        ret = ROUTER_ERR_IMPLEMENTED;
     969                }
     970        }
     971
     972        return ret;
     973}
     974
     975int router_hwinfo_get(void *d, struct router_hw_info *hwinfo)
     976{
     977        struct router_priv *priv = d;
     978        int error = router_check_open(d);
     979
     980        if (error)
     981                return error;
     982
     983        if (hwinfo == NULL) {
     984                DBG("ROUTER Wrong pointer\n");
     985                return ROUTER_ERR_EINVAL;
     986        }
     987
     988        /* Get hwinfo */
     989        router_hwinfo(priv, hwinfo);
     990
     991        return ROUTER_ERR_OK;
     992}
     993
     994int router_print(void *d)
     995{
     996        struct router_priv *priv = d;
     997        int error = router_check_open(d);
     998
     999        if (error)
     1000                return error;
     1001
     1002        /* DEBUG print */
     1003        DBG("Number of routers: %d\n", router_count);
     1004        DBG("SPW ROUTER[%d] with following capabilities:\n", priv->index);
     1005        router_hwinfo_print(&priv->hwinfo);
     1006
     1007        return ROUTER_ERR_OK;
     1008}
     1009
     1010/* Configure Router. Leave field NULL in order to skip configuration
     1011 */
     1012int router_config_set(void *d, struct router_config *cfg)
     1013{
     1014        struct router_priv *priv = d;
     1015        int error = router_check_open(d);
     1016
     1017        if (error)
     1018                return error;
     1019
     1020        if (cfg == NULL) {
     1021                DBG("ROUTER CFG wrong\n");
     1022                return ROUTER_ERR_EINVAL;
     1023        }
     1024
     1025        if ((cfg->flags & (ROUTER_FLG_TPRES|ROUTER_FLG_TRLD)) &&
     1026                        !priv->hwinfo.timers_avail) {
     1027                return ROUTER_ERR_IMPLEMENTED;
     1028        }
     1029
     1030        #ifdef THREAD_SAFE
     1031        /* Take device lock - Wait until we get semaphore */
     1032        if (rtems_semaphore_obtain(priv->sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
     1033                        != RTEMS_SUCCESSFUL) {
     1034                DBG("ROUTER Sempahore failed\n");
     1035                return ROUTER_ERR_ERROR;
     1036        }
     1037        #endif
     1038
     1039        /* Write only configuration bits in Config register */
     1040        if (cfg->flags & ROUTER_FLG_CFG) {
     1041                REG_WRITE(&priv->regs->cfgsts, cfg->config & ~(RTRCFG_WCLEAR));
     1042        }
     1043
     1044        /* Write Instance ID to Version Register */
     1045        if (cfg->flags & ROUTER_FLG_IID) {
     1046                REG_WRITE(&priv->regs->ver, (cfg->iid << VER_ID_BIT) & VER_ID);
     1047        }
     1048
     1049        /* Write startup-clock-divisor Register */
     1050        if (cfg->flags & ROUTER_FLG_IDIV) {
     1051                REG_WRITE(&priv->regs->idiv, (cfg->idiv << IDIV_ID_BIT) & IDIV_ID);
     1052        }
     1053
     1054        /* Write Timer Prescaler Register */
     1055        if (cfg->flags & ROUTER_FLG_TPRES) {
     1056                REG_WRITE(&priv->regs->tprescaler,
     1057                                (cfg->timer_prescaler << PRESCALER_RL_BIT) & PRESCALER_RL);
     1058        }
     1059
     1060        #ifdef THREAD_SAFE
     1061        /* Unlock dev */
     1062        rtems_semaphore_release(priv->sem);
     1063        #endif
     1064
     1065        return ROUTER_ERR_OK;
     1066}
     1067
     1068int router_config_get(void *d, struct router_config *cfg)
     1069{
     1070        struct router_priv *priv = d;
     1071        int error = router_check_open(d);
     1072
     1073        if (error)
     1074                return error;
     1075
     1076        if (cfg == NULL) {
     1077                DBG("ROUTER CFG wrong\n");
     1078                return ROUTER_ERR_EINVAL;
     1079        }
     1080
     1081        #ifdef THREAD_SAFE
     1082        /* Take device lock - Wait until we get semaphore */
     1083        if (rtems_semaphore_obtain(priv->sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
     1084                        != RTEMS_SUCCESSFUL) {
     1085                DBG("ROUTER Sempahore failed\n");
     1086                return ROUTER_ERR_ERROR;
     1087        }
     1088        #endif
     1089
     1090        cfg->config = REG_READ(&priv->regs->cfgsts) &
     1091                ~(RTRCFG_SP|RTRCFG_AP|RTRCFG_FP|RTRCFG_SR|RTRCFG_PE|RTRCFG_ME|
     1092                  RTRCFG_TA|RTRCFG_PP);
     1093        cfg->iid = (REG_READ(&priv->regs->ver) & VER_ID) >> VER_ID_BIT;
     1094        cfg->idiv = (REG_READ(&priv->regs->idiv) & IDIV_ID) >> IDIV_ID_BIT;
     1095        cfg->timer_prescaler =
     1096                (REG_READ(&priv->regs->tprescaler) & PRESCALER_RL) >> PRESCALER_RL_BIT;
     1097
     1098        #ifdef THREAD_SAFE
     1099        /* Unlock dev */
     1100        rtems_semaphore_release(priv->sem);
     1101        #endif
     1102
     1103        return ROUTER_ERR_OK;
     1104}
     1105
     1106/* Configure Router routing table.
     1107 * Leave field NULL in order to skip configuration
     1108 */
     1109int router_routing_table_set(void *d, struct router_routing_table *cfg)
     1110{
     1111        struct router_priv *priv = d;
     1112        int error = router_check_open(d);
     1113
     1114        if (error)
     1115                return error;
     1116
     1117        if (cfg == NULL) {
     1118                DBG("ROUTER CFG wrong\n");
     1119                return ROUTER_ERR_EINVAL;
     1120        }
     1121
     1122        #ifdef THREAD_SAFE
     1123        /* Take device lock - Wait until we get semaphore */
     1124        if (rtems_semaphore_obtain(priv->sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
     1125                        != RTEMS_SUCCESSFUL) {
     1126                DBG("ROUTER Sempahore failed\n");
     1127                return ROUTER_ERR_ERROR;
     1128        }
     1129        #endif
     1130
     1131        /* Write Address control */
     1132        if (cfg->flags & ROUTER_ROUTE_FLG_CTRL) {
     1133                router_acontrol_set(priv,&cfg->acontrol);
     1134        }
     1135
     1136        /* Write Port map */
     1137        if (cfg->flags & ROUTER_ROUTE_FLG_MAP) {
     1138                router_portmap_set(priv,&cfg->portmap);
     1139        }
     1140
     1141        #ifdef THREAD_SAFE
     1142        /* Unlock dev */
     1143        rtems_semaphore_release(priv->sem);
     1144        #endif
     1145
     1146        return ROUTER_ERR_OK;
     1147}
     1148
     1149int router_routing_table_get(void *d, struct router_routing_table *cfg)
     1150{
     1151        struct router_priv *priv = d;
     1152        int error = router_check_open(d);
     1153
     1154        if (error)
     1155                return error;
     1156
     1157        if (cfg == NULL) {
     1158                DBG("ROUTER CFG wrong\n");
     1159                return ROUTER_ERR_EINVAL;
     1160        }
     1161
     1162        #ifdef THREAD_SAFE
     1163        /* Take device lock - Wait until we get semaphore */
     1164        if (rtems_semaphore_obtain(priv->sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
     1165                        != RTEMS_SUCCESSFUL) {
     1166                DBG("ROUTER Sempahore failed\n");
     1167                return ROUTER_ERR_ERROR;
     1168        }
     1169        #endif
     1170
     1171        /* Read Address control */
     1172        router_acontrol_get(priv,&cfg->acontrol);
     1173
     1174        /* Read Port map */
     1175        router_portmap_get(priv,&cfg->portmap);
     1176
     1177        #ifdef THREAD_SAFE
     1178        /* Unlock dev */
     1179        rtems_semaphore_release(priv->sem);
     1180        #endif
     1181
     1182        return ROUTER_ERR_OK;
     1183}
     1184
     1185int router_route_set(void *d, struct router_route *route)
     1186{
     1187        struct router_priv *priv = d;
    3071188        int i;
    308 
    309         if ( (cfg->flags & (ROUTER_FLG_TPRES|ROUTER_FLG_TRLD)) &&
    310              !priv->hwinfo.timers_avail ) {
    311                 return RTEMS_NOT_IMPLEMENTED;
    312         }
    313 
    314         /* Write only configuration bits in Config register */
    315         if ( cfg->flags & ROUTER_FLG_CFG ) {
    316                 REG_WRITE(&priv->regs->cfgsts, cfg->config & ~0x4);
    317         }
    318 
    319         /* Write Instance ID to Version Register */
    320         if ( cfg->flags & ROUTER_FLG_IID ) {
    321                 REG_WRITE(&priv->regs->ver, cfg->iid);
    322         }
    323 
    324         /* Write startup-clock-divisor Register */
    325         if ( cfg->flags & ROUTER_FLG_IDIV ) {
    326                 REG_WRITE(&priv->regs->idiv, cfg->idiv);
    327         }
    328 
    329         /* Write Timer Prescaler Register */
    330         if ( cfg->flags & ROUTER_FLG_TPRES ) {
    331                 REG_WRITE(&priv->regs->tprescaler, cfg->timer_prescaler);
    332         }
    333 
    334         /* Write Timer Reload Register */
    335         if ( cfg->flags & ROUTER_FLG_TRLD ) {
    336                 for (i=0; i<=priv->nports; i++)
    337                         REG_WRITE(&priv->regs->treload[i], cfg->timer_reload[i]);
    338         }
    339 
    340         return 0;
    341 }
    342 
    343 static int router_config_read(
    344         struct router_priv *priv,
    345         struct router_config *cfg)
    346 {
    347         int i;
    348 
    349         cfg->config = REG_READ(&priv->regs->cfgsts) & ~0xffff0007;
    350         cfg->iid = REG_READ(&priv->regs->ver) & 0xff;
    351         cfg->idiv = REG_READ(&priv->regs->idiv) & 0xff;
    352         cfg->timer_prescaler = REG_READ(&priv->regs->tprescaler);
    353         for (i=0; i<=priv->nports; i++)
    354                 cfg->timer_reload[i] = REG_READ(&priv->regs->treload[i]);
    355 
    356         return 0;
    357 }
    358 
    359 static int router_routes_set(
    360         struct router_priv *priv,
    361         struct router_routes *routes)
    362 {
    363         int i;
    364         for (i=0; i<224; i++)
    365                 REG_WRITE(&priv->regs->routes[i], routes->route[i]);
    366         return 0;
    367 }
    368 
    369 static int router_routes_read(
    370         struct router_priv *priv,
    371         struct router_routes *routes)
    372 {
    373         int i;
    374         for (i=0; i<224; i++)
    375                 routes->route[i] = REG_READ(&priv->regs->routes[i]);
    376         return 0;
    377 }
    378 
    379 static int router_ps_set(struct router_priv *priv, struct router_ps *ps)
    380 {
    381         int i;
    382         unsigned int *p = &ps->ps[0];
    383         for (i=0; i<255; i++,p++)
    384                 REG_WRITE(&priv->regs->psetup[i], *p);
    385         return 0;
    386 }
    387 
    388 static int router_ps_read(struct router_priv *priv, struct router_ps *ps)
    389 {
    390         int i;
    391         unsigned int *p = &ps->ps[0];
    392         for (i=0; i<255; i++,p++)
    393                 REG_WRITE(&priv->regs->psetup[i], *p);
    394         return 0;
    395 }
    396 
    397 static int router_we_set(struct router_priv *priv, int we)
    398 {
    399         REG_WRITE(&priv->regs->cfgwe, we & 0x1);
    400         return 0;
    401 }
    402 
    403 static int router_port_ctrl(struct router_priv *priv, struct router_port *port)
    404 {
    405         unsigned int ctrl, sts;
    406 
    407         if ( port->port > priv->nports )
    408                 return RTEMS_INVALID_NAME;
    409 
    410         ctrl = port->ctrl;
    411         if ( port->flag & ROUTER_PORTFLG_GET_CTRL ) {
    412                 ctrl = REG_READ(&priv->regs->pctrl[port->port]);
    413         }
    414         sts = port->sts;
    415         if ( port->flag & ROUTER_PORTFLG_GET_STS ) {
    416                 sts = REG_READ(&priv->regs->psts[port->port]);
    417         }
    418 
    419         if ( port->flag & ROUTER_PORTFLG_SET_CTRL ) {
    420                 REG_WRITE(&priv->regs->pctrl[port->port], port->ctrl);
    421         }
    422         if ( port->flag & ROUTER_PORTFLG_SET_STS ) {
    423                 REG_WRITE(&priv->regs->psts[port->port], port->sts);
    424         }
    425 
    426         port->ctrl = ctrl;
    427         port->sts = sts;
    428         return 0;
    429 }
    430 
    431 static int router_cfgsts_set(struct router_priv *priv, unsigned int cfgsts)
    432 {
     1189        unsigned int mask;
     1190        int error = router_check_open(d);
     1191
     1192        if (error)
     1193                return error;
     1194
     1195        if (route == NULL) {
     1196                DBG("ROUTER route wrong\n");
     1197                return ROUTER_ERR_EINVAL;
     1198        }
     1199
     1200        if (route->from_address < 32) {
     1201                /* Physical address */
     1202                if ((route->from_address == 0) ||
     1203                                (route->from_address >= priv->nports)) {
     1204                        DBG("ROUTER wrong physical address\n");
     1205                        return ROUTER_ERR_TOOMANY;
     1206                }
     1207        }
     1208
     1209        /* Compute port map */
     1210        mask=0;
     1211        for (i=0; i < route->count; i++) {
     1212                if ((route->to_port[i] == 0) || (route->to_port[i] >= priv->nports)) {
     1213                        DBG("ROUTER route wrong destiny port\n");
     1214                        return ROUTER_ERR_EINVAL;
     1215                }
     1216                mask |= (0x1 << route->to_port[i]);
     1217        }
     1218        if (route->options & ROUTER_ROUTE_PACKETDISTRIBUTION_ENABLE) {
     1219                mask |= RTPMAP_PD;
     1220        }
     1221
     1222        #ifdef THREAD_SAFE
     1223        /* Take device lock - Wait until we get semaphore */
     1224        if (rtems_semaphore_obtain(priv->sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
     1225                        != RTEMS_SUCCESSFUL) {
     1226                DBG("ROUTER Sempahore failed\n");
     1227                return ROUTER_ERR_ERROR;
     1228        }
     1229        #endif
     1230
     1231        /* Write port map */
     1232        REG_WRITE(&priv->regs->psetup[route->from_address-1], mask);
     1233
     1234        /* Write Address control */
     1235        REG_WRITE(&priv->regs->routes[route->from_address-1],
     1236                        route->options & (0xf));
     1237
     1238        #ifdef THREAD_SAFE
     1239        /* Unlock dev */
     1240        rtems_semaphore_release(priv->sem);
     1241        #endif
     1242
     1243        return ROUTER_ERR_OK;
     1244}
     1245
     1246int router_route_get(void *d, struct router_route *route)
     1247{
     1248        struct router_priv *priv = d;
     1249        int i,count;
     1250        unsigned int mask;
     1251        int error = router_check_open(d);
     1252
     1253        if (error)
     1254                return error;
     1255
     1256        if (route == NULL) {
     1257                DBG("ROUTER route wrong\n");
     1258                return ROUTER_ERR_EINVAL;
     1259        }
     1260
     1261        if (route->from_address < 32) {
     1262                /* Physical address */
     1263                if ((route->from_address == 0) ||
     1264                                (route->from_address >= priv->nports)) {
     1265                        DBG("ROUTER wrong physical address\n");
     1266                        return ROUTER_ERR_TOOMANY;
     1267                }
     1268        }
     1269
     1270        #ifdef THREAD_SAFE
     1271        /* Take device lock - Wait until we get semaphore */
     1272        if (rtems_semaphore_obtain(priv->sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
     1273                        != RTEMS_SUCCESSFUL) {
     1274                DBG("ROUTER Sempahore failed\n");
     1275                return ROUTER_ERR_ERROR;
     1276        }
     1277        #endif
     1278
     1279        /* Get Address control */
     1280        route->options =
     1281                REG_READ(&priv->regs->routes[route->from_address-1]) & (0xf);
     1282
     1283        /* Read port map */
     1284        mask=REG_READ(&priv->regs->psetup[route->from_address-1]);
     1285
     1286        #ifdef THREAD_SAFE
     1287        /* Unlock dev */
     1288        rtems_semaphore_release(priv->sem);
     1289        #endif
     1290
     1291        if (mask & RTPMAP_PD) {
     1292                route->options |= ROUTER_ROUTE_PACKETDISTRIBUTION_ENABLE;
     1293        }
     1294
     1295        /*DBG("ROUTE from address 0x%02x read, PMAP: 0x%08x, CTRL: 0x%08x\n",
     1296         *              (unsigned int) route->from_address, mask,
     1297         *              (unsigned int) route->options);*/
     1298
     1299        i=0;
     1300        count=0;
     1301        mask &= (RTPMAP_PE);
     1302        while (mask != 0) {
     1303                if (mask & 0x1) {
     1304                        route->to_port[count] = i;
     1305                        count++;
     1306                }
     1307                mask >>= 1;
     1308                i++;
     1309        }
     1310        route->count=count;
     1311
     1312        return ROUTER_ERR_OK;
     1313}
     1314
     1315int router_write_enable(void *d)
     1316{
     1317        struct router_priv *priv = d;
     1318        int error = router_check_open(d);
     1319
     1320        if (error)
     1321                return error;
     1322
     1323        REG_WRITE(&priv->regs->cfgwe, 0x1);
     1324
     1325        return ROUTER_ERR_OK;
     1326}
     1327
     1328int router_write_disable(void *d)
     1329{
     1330        struct router_priv *priv = d;
     1331        int error = router_check_open(d);
     1332
     1333        if (error)
     1334                return error;
     1335
     1336        REG_WRITE(&priv->regs->cfgwe, 0x0);
     1337
     1338        return ROUTER_ERR_OK;
     1339}
     1340
     1341int router_port_ioc(void *d, int port, struct router_port *cfg)
     1342{
     1343        struct router_priv *priv = d;
     1344        unsigned int ctrl, ctrl2, sts, timer, pktl;
     1345        SPIN_IRQFLAGS(irqflags);
     1346        int error = router_check_port(d, port);
     1347
     1348        if (error)
     1349                return error;
     1350
     1351        if (cfg == NULL) {
     1352                DBG("ROUTER Wrong cfg\n");
     1353                return ROUTER_ERR_EINVAL;
     1354        }
     1355
     1356        SPIN_LOCK_IRQ(&priv->plock[port], irqflags);
     1357
     1358        ctrl = cfg->ctrl;
     1359        if (cfg->flag & ROUTER_PORT_FLG_GET_CTRL) {
     1360                ctrl = REG_READ(&priv->regs->pctrl[port]);
     1361        }
     1362        ctrl2 = cfg->ctrl;
     1363        if (cfg->flag & ROUTER_PORT_FLG_GET_CTRL2) {
     1364                ctrl2 = REG_READ(&priv->regs->pctrl2[port]);
     1365        }
     1366        sts = cfg->sts;
     1367        if (cfg->flag & ROUTER_PORT_FLG_GET_STS) {
     1368                sts = REG_READ(&priv->regs->psts[port]);
     1369        }
     1370        timer = cfg->timer_reload;
     1371        if (cfg->flag & ROUTER_PORT_FLG_GET_TIMER) {
     1372                REG_READ(&priv->regs->treload[port]);
     1373        }
     1374        pktl = cfg->packet_length;
     1375        if (cfg->flag & ROUTER_PORT_FLG_GET_PKTLEN) {
     1376                REG_READ(&priv->regs->maxplen[port]);
     1377        }
     1378
     1379        if (cfg->flag & ROUTER_PORT_FLG_SET_CTRL) {
     1380                REG_WRITE(&priv->regs->pctrl[port], cfg->ctrl);
     1381        }
     1382        if (cfg->flag & ROUTER_PORT_FLG_SET_CTRL2) {
     1383                REG_WRITE(&priv->regs->pctrl2[port], cfg->ctrl2);
     1384        }
     1385        if (cfg->flag & ROUTER_PORT_FLG_SET_STS) {
     1386                REG_WRITE(&priv->regs->psts[port], cfg->sts);
     1387        }
     1388        if (cfg->flag & ROUTER_PORT_FLG_SET_TIMER) {
     1389                REG_WRITE(&priv->regs->treload[port], cfg->timer_reload & PTIMER_RL);
     1390        }
     1391        if (cfg->flag & ROUTER_PORT_FLG_SET_PKTLEN) {
     1392                REG_WRITE(&priv->regs->maxplen[port], cfg->packet_length & MAXPLEN_ML);
     1393        }
     1394
     1395        SPIN_UNLOCK_IRQ(&priv->plock[port], irqflags);
     1396
     1397        cfg->ctrl = ctrl;
     1398        cfg->ctrl2 = ctrl2;
     1399        cfg->sts = sts;
     1400        cfg->timer_reload = timer;
     1401        cfg->packet_length = pktl;
     1402
     1403        return ROUTER_ERR_OK;
     1404}
     1405
     1406/* Read Port Control register */
     1407int router_port_ctrl_get(void *d, int port, uint32_t *ctrl)
     1408{
     1409        struct router_priv *priv = d;
     1410        int error = router_check_port(d, port);
     1411
     1412        if (error)
     1413                return error;
     1414
     1415        if (ctrl == NULL) {
     1416                DBG("ROUTER Wrong ctrl\n");
     1417                return ROUTER_ERR_EINVAL;
     1418        }
     1419
     1420        *ctrl = REG_READ(&priv->regs->pctrl[port]);
     1421        return ROUTER_ERR_OK;
     1422}
     1423
     1424/* Read Port Status register and clear errors if there are */
     1425int router_port_status(void *d, int port, uint32_t *sts, uint32_t clrmsk) /* review clrmsks */
     1426{
     1427        struct router_priv *priv = d;
     1428        int error = router_check_port(d, port);
     1429
     1430        if (error)
     1431                return error;
     1432
     1433        if (sts == NULL) {
     1434                DBG("ROUTER Wrong sts\n");
     1435                return ROUTER_ERR_EINVAL;
     1436        }
     1437        *sts = REG_READ(&priv->regs->psts[port]);
     1438        if (port == 0) {
     1439                REG_WRITE(&priv->regs->psts[port], (*sts) & PSTSCFG_WCLEAR);
     1440        }else{
     1441                REG_WRITE(&priv->regs->psts[port], (*sts) & PSTS_WCLEAR);
     1442        }
     1443        return ROUTER_ERR_OK;
     1444}
     1445
     1446/* Read Port Control2 register */
     1447int router_port_ctrl2_get(void *d, int port, uint32_t *ctrl2)
     1448{
     1449        struct router_priv *priv = d;
     1450        int error = router_check_port(d, port);
     1451
     1452        if (error)
     1453                return error;
     1454
     1455        if (ctrl2 == NULL) {
     1456                DBG("ROUTER Wrong ctrl2\n");
     1457                return ROUTER_ERR_EINVAL;
     1458        }
     1459
     1460        *ctrl2 = REG_READ(&priv->regs->pctrl2[port]);
     1461        return ROUTER_ERR_OK;
     1462}
     1463
     1464/* Write Port Control Register */
     1465int router_port_ctrl_set(void *d, int port, uint32_t ctrl)
     1466{
     1467        struct router_priv *priv = d;
     1468        SPIN_IRQFLAGS(irqflags);
     1469        int error = router_check_port(d, port);
     1470
     1471        if (error)
     1472                return error;
     1473
     1474        SPIN_LOCK_IRQ(&priv->plock[port], irqflags);
     1475
     1476        REG_WRITE(&priv->regs->pctrl[port],ctrl); /* this is not SMP safe? */
     1477
     1478        SPIN_UNLOCK_IRQ(&priv->plock[port], irqflags);
     1479
     1480        return ROUTER_ERR_OK;
     1481}
     1482
     1483/* Write Port Control2 Register */
     1484int router_port_ctrl2_set(void *d, int port, uint32_t ctrl2)
     1485{
     1486        struct router_priv *priv = d;
     1487        int error = router_check_port(d, port);
     1488
     1489        if (error)
     1490                return error;
     1491
     1492        REG_WRITE(&priv->regs->pctrl2[port],ctrl2);
     1493
     1494        return ROUTER_ERR_OK;
     1495}
     1496
     1497int router_port_treload_set(void *d, int port, uint32_t reload)
     1498{
     1499        struct router_priv *priv = d;
     1500        int error = router_check_port(d, port);
     1501
     1502        if (error)
     1503                return error;
     1504
     1505        REG_WRITE(&priv->regs->treload[port], reload & PTIMER_RL);
     1506
     1507        return ROUTER_ERR_OK;
     1508}
     1509
     1510int router_port_treload_get(void *d, int port, uint32_t *reload)
     1511{
     1512        struct router_priv *priv = d;
     1513        int error = router_check_port(d, port);
     1514
     1515        if (error)
     1516                return error;
     1517
     1518        if (reload == NULL) {
     1519                DBG("ROUTER Wrong reload pointer\n");
     1520                return ROUTER_ERR_EINVAL;
     1521        }
     1522
     1523        *reload = REG_READ(&priv->regs->treload[port]) & PTIMER_RL;
     1524
     1525        return ROUTER_ERR_OK;
     1526}
     1527
     1528int router_port_maxplen_set(void *d, int port, uint32_t length)
     1529{
     1530        struct router_priv *priv = d;
     1531        int error = router_check_port(d, port);
     1532
     1533        if (error)
     1534                return error;
     1535
     1536        REG_WRITE(&priv->regs->maxplen[port], length & MAXPLEN_ML);
     1537
     1538        return ROUTER_ERR_OK;
     1539}
     1540
     1541int router_port_maxplen_get(void *d, int port, uint32_t *length)
     1542{
     1543        struct router_priv *priv = d;
     1544        int error = router_check_port(d, port);
     1545
     1546        if (error)
     1547                return error;
     1548
     1549        if (length == NULL) {
     1550                DBG("ROUTER Wrong length pointer\n");
     1551                return ROUTER_ERR_EINVAL;
     1552        }
     1553
     1554        *length = REG_READ(&priv->regs->maxplen[port]);
     1555
     1556        return ROUTER_ERR_OK;
     1557}
     1558
     1559/* Get Port Link Status */
     1560int router_port_link_status(void *d, int port)
     1561{
     1562        struct router_priv *priv = d;
     1563        int error = router_check_port(d, port);
     1564
     1565        if (error)
     1566                return error;
     1567
     1568        return ((REG_READ(&priv->regs->psts[port]) & PSTS_LS) >> PSTS_LS_BIT);
     1569}
     1570
     1571int router_port_disable(void *d, int port)
     1572{
     1573        struct router_priv *priv = d;
     1574        unsigned int ctrl;
     1575        SPIN_IRQFLAGS(irqflags);
     1576        int error = router_check_port(d, port);
     1577
     1578        if (error)
     1579                return error;
     1580
     1581        SPIN_LOCK_IRQ(&priv->plock[port], irqflags);
     1582
     1583        ctrl = REG_READ(&priv->regs->pctrl[port]);
     1584        REG_WRITE(&priv->regs->pctrl[port], (ctrl | PCTRL_DI));
     1585
     1586        SPIN_UNLOCK_IRQ(&priv->plock[port], irqflags);
     1587
     1588        return ROUTER_ERR_OK;
     1589}
     1590
     1591int router_port_enable(void *d, int port)
     1592{
     1593        struct router_priv *priv = d;
     1594        unsigned int ctrl;
     1595        SPIN_IRQFLAGS(irqflags);
     1596        int error = router_check_port(d, port);
     1597
     1598        if (error)
     1599                return error;
     1600
     1601        SPIN_LOCK_IRQ(&priv->plock[port], irqflags);
     1602
     1603        ctrl = REG_READ(&priv->regs->pctrl[port]);
     1604        REG_WRITE(&priv->regs->pctrl[port], (ctrl & ~(PCTRL_DI)));
     1605
     1606        SPIN_UNLOCK_IRQ(&priv->plock[port], irqflags);
     1607
     1608        return ROUTER_ERR_OK;
     1609}
     1610
     1611int router_port_link_stop(void *d, int port)
     1612{
     1613        struct router_priv *priv = d;
     1614        unsigned int ctrl;
     1615        SPIN_IRQFLAGS(irqflags);
     1616        int error = router_check_port(d, port);
     1617
     1618        if (error)
     1619                return error;
     1620
     1621        SPIN_LOCK_IRQ(&priv->plock[port], irqflags);
     1622
     1623        ctrl = REG_READ(&priv->regs->pctrl[port]);
     1624        REG_WRITE(&priv->regs->pctrl[port], ((ctrl & ~(PCTRL_LS) )| (PCTRL_LD)));
     1625
     1626        SPIN_UNLOCK_IRQ(&priv->plock[port], irqflags);
     1627
     1628        return ROUTER_ERR_OK;
     1629}
     1630
     1631int router_port_link_start(void *d, int port)
     1632{
     1633        struct router_priv *priv = d;
     1634        unsigned int ctrl;
     1635        SPIN_IRQFLAGS(irqflags);
     1636        int error = router_check_port(d, port);
     1637
     1638        if (error)
     1639                return error;
     1640
     1641        SPIN_LOCK_IRQ(&priv->plock[port], irqflags);
     1642
     1643        ctrl = REG_READ(&priv->regs->pctrl[port]);
     1644        REG_WRITE(&priv->regs->pctrl[port], ((ctrl & ~(PCTRL_LD) )| (PCTRL_LS)));
     1645
     1646        SPIN_UNLOCK_IRQ(&priv->plock[port], irqflags);
     1647
     1648        return ROUTER_ERR_OK;
     1649}
     1650
     1651int router_port_link_receive_spill(void *d, int port)
     1652{
     1653        struct router_priv *priv = d;
     1654        unsigned int ctrl;
     1655        SPIN_IRQFLAGS(irqflags);
     1656        int error = router_check_port(d, port);
     1657
     1658        if (error)
     1659                return error;
     1660
     1661        SPIN_LOCK_IRQ(&priv->plock[port], irqflags);
     1662
     1663        ctrl = REG_READ(&priv->regs->pctrl[port]);
     1664        REG_WRITE(&priv->regs->pctrl[port], (ctrl| (PCTRL_RS)));
     1665
     1666        /* Wait until the spill is done */
     1667        while(REG_READ(&priv->regs->pctrl[port]) & PCTRL_RS) {};
     1668
     1669        SPIN_UNLOCK_IRQ(&priv->plock[port], irqflags);
     1670
     1671        return ROUTER_ERR_OK;
     1672}
     1673
     1674int router_port_link_transmit_reset(void *d, int port)
     1675{
     1676        struct router_priv *priv = d;
     1677        unsigned int ctrl;
     1678        SPIN_IRQFLAGS(irqflags);
     1679        int error = router_check_port(d, port);
     1680
     1681        if (error)
     1682                return error;
     1683
     1684        SPIN_LOCK_IRQ(&priv->plock[port], irqflags);
     1685
     1686        ctrl = REG_READ(&priv->regs->pctrl[port]);
     1687        REG_WRITE(&priv->regs->pctrl[port], (ctrl| (PCTRL_TF)));
     1688
     1689        /* Wait until the spill is done */
     1690        while(REG_READ(&priv->regs->pctrl[port]) & PCTRL_TF) {};
     1691
     1692        SPIN_UNLOCK_IRQ(&priv->plock[port], irqflags);
     1693
     1694        return ROUTER_ERR_OK;
     1695}
     1696
     1697int router_port_cred_get(void *d, int port, uint32_t *cred)
     1698{
     1699        struct router_priv *priv = d;
     1700        int error = router_check_port(d, port);
     1701
     1702        if (error)
     1703                return error;
     1704
     1705        if (cred == NULL) {
     1706                DBG("ROUTER Wrong cred pointer\n");
     1707                return ROUTER_ERR_EINVAL;
     1708        }
     1709
     1710        *cred = REG_READ(&priv->regs->credcnt[port]);
     1711        return ROUTER_ERR_OK;
     1712}
     1713
     1714int router_instance_set(void *d, uint8_t instance)
     1715{
     1716        struct router_priv *priv = d;
     1717        int error = router_check_open(d);
     1718
     1719        if (error)
     1720                return error;
     1721
     1722        REG_WRITE(&priv->regs->ver, (instance << VER_ID_BIT) & VER_ID);
     1723        return ROUTER_ERR_OK;
     1724}
     1725
     1726int router_idiv_set(void *d, uint8_t idiv)
     1727{
     1728        struct router_priv *priv = d;
     1729        int error = router_check_open(d);
     1730
     1731        if (error)
     1732                return error;
     1733
     1734        REG_WRITE(&priv->regs->idiv, (idiv << IDIV_ID_BIT) & IDIV_ID);
     1735        return ROUTER_ERR_OK;
     1736}
     1737
     1738int router_tpresc_set(void *d, uint32_t prescaler)
     1739{
     1740        struct router_priv *priv = d;
     1741        int error = router_check_open(d);
     1742
     1743        if (error)
     1744                return error;
     1745
     1746        REG_WRITE(&priv->regs->tprescaler,
     1747                        (prescaler << PRESCALER_RL_BIT) & PRESCALER_RL);
     1748        return ROUTER_ERR_OK;
     1749}
     1750
     1751int router_instance_get(void *d, uint8_t *instance)
     1752{
     1753        struct router_priv *priv = d;
     1754        int error = router_check_open(d);
     1755
     1756        if (error)
     1757                return error;
     1758
     1759        if (instance == NULL) {
     1760                DBG("ROUTER Wrong instance pointer\n");
     1761                return ROUTER_ERR_EINVAL;
     1762        }
     1763
     1764        *instance = REG_READ(&priv->regs->ver);
     1765
     1766        return ROUTER_ERR_OK;
     1767}
     1768
     1769int router_idiv_get(void *d, uint8_t *idiv)
     1770{
     1771        struct router_priv *priv = d;
     1772        int error = router_check_open(d);
     1773
     1774        if (error)
     1775                return error;
     1776
     1777        if (idiv == NULL) {
     1778                DBG("ROUTER Wrong idiv pointer\n");
     1779                return ROUTER_ERR_EINVAL;
     1780        }
     1781
     1782        *idiv = REG_READ(&priv->regs->idiv);
     1783
     1784        return ROUTER_ERR_OK;
     1785}
     1786
     1787int router_tpresc_get(void *d, uint32_t *prescaler)
     1788{
     1789        struct router_priv *priv = d;
     1790        int error = router_check_open(d);
     1791
     1792        if (error)
     1793                return error;
     1794
     1795        if (prescaler == NULL) {
     1796                DBG("ROUTER Wrong prescaler pointer\n");
     1797                return ROUTER_ERR_EINVAL;
     1798        }
     1799
     1800        *prescaler = REG_READ(&priv->regs->tprescaler);
     1801
     1802        return ROUTER_ERR_OK;
     1803}
     1804
     1805int router_cfgsts_set(void *d, uint32_t cfgsts)
     1806{
     1807        struct router_priv *priv = d;
     1808        int error = router_check_open(d);
     1809
     1810        if (error)
     1811                return error;
     1812
    4331813        REG_WRITE(&priv->regs->cfgsts, cfgsts);
    434         return 0;
    435 }
    436 
    437 static int router_cfgsts_read(struct router_priv *priv, unsigned int *cfgsts)
    438 {
     1814
     1815        return ROUTER_ERR_OK;
     1816}
     1817
     1818int router_cfgsts_get(void *d, uint32_t *cfgsts)
     1819{
     1820        struct router_priv *priv = d;
     1821        int error = router_check_open(d);
     1822
     1823        if (error)
     1824                return error;
     1825
     1826        if (cfgsts == NULL) {
     1827                DBG("ROUTER Wrong cfgsts pointer\n");
     1828                return ROUTER_ERR_EINVAL;
     1829        }
     1830
    4391831        *cfgsts = REG_READ(&priv->regs->cfgsts);
    440         return 0;
    441 }
    442 
    443 static int router_tc_read(struct router_priv *priv, unsigned int *tc)
    444 {
    445         *tc = REG_READ(&priv->regs->timecode);
    446         return 0;
    447 }
    448 
    449 static rtems_device_driver router_control(
    450         rtems_device_major_number major,
    451         rtems_device_minor_number minor,
    452         void                    * arg
    453         )
    454 {
    455         struct router_priv *priv;
    456         struct drvmgr_dev *dev;
    457         rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t *) arg;
    458         void *argp = (void *)ioarg->buffer;
    459 
    460         if ( drvmgr_get_dev(&router_drv_info.general, minor, &dev) ) {
    461                 ROUTER_DBG("Wrong minor %d\n", minor);
    462                 return RTEMS_INVALID_NAME;
    463         }
    464         priv = (struct router_priv *)dev->priv;
    465 
    466         ioarg->ioctl_return = 0;
    467         switch (ioarg->command) {
    468 
    469         /* Get Hardware support/information available */
    470         case GRSPWR_IOCTL_HWINFO:
    471         {
    472                 struct router_hw_info *hwinfo = argp;
    473                 router_hwinfo(priv, hwinfo);
    474                 break;
    475         }
    476 
    477         /* Set Router Configuration */
    478         case GRSPWR_IOCTL_CFG_SET:
    479         {
    480                 struct router_config *cfg = argp;
    481                 return router_config_set(priv, cfg);
    482         }
    483 
    484         /* Read Router Configuration */
    485         case GRSPWR_IOCTL_CFG_GET:
    486         {
    487                 struct router_config *cfg = argp;
    488                 router_config_read(priv, cfg);
    489                 break;
    490         }
    491 
    492         /* Routes */
    493         case GRSPWR_IOCTL_ROUTES_SET:
    494         {
    495                 struct router_routes *routes = argp;
    496                 return router_routes_set(priv, routes);
    497         }
    498 
    499         case GRSPWR_IOCTL_ROUTES_GET:
    500         {
    501                 struct router_routes *routes = argp;
    502                 router_routes_read(priv, routes);
    503                 break;
    504         }
    505 
    506         /* Port Setup */
    507         case GRSPWR_IOCTL_PS_SET:
    508         {
    509                 struct router_ps *ps = argp;
    510                 return router_ps_set(priv, ps);
    511         }
    512 
    513         case GRSPWR_IOCTL_PS_GET:
    514         {
    515                 struct router_ps *ps = argp;
    516                 router_ps_read(priv, ps);
    517                 break;
    518         }
    519 
    520         /* Set configuration write enable */
    521         case GRSPWR_IOCTL_WE_SET:
    522         {
    523                 return router_we_set(priv, (int)argp);
    524         }
    525 
    526         /* Set/Get Port Control/Status */
    527         case GRSPWR_IOCTL_PORT:
    528         {
    529                 struct router_port *port = argp;
    530                 int result;
    531                 if ( (result=router_port_ctrl(priv, port)) )
    532                         return result;
    533                 break;
    534         }
    535 
    536         /* Set Router Configuration/Status Register */
    537         case GRSPWR_IOCTL_CFGSTS_SET:
    538         {
    539                 return router_cfgsts_set(priv, (int)argp);
    540         }
    541 
    542         /* Get Router Configuration/Status Register */
    543         case GRSPWR_IOCTL_CFGSTS_GET:
    544         {
    545                 unsigned int *cfgsts = argp;
    546                 router_cfgsts_read(priv, cfgsts);
    547                 break;
    548         }
    549 
    550         /* Get Current Time-Code Register */
    551         case GRSPWR_IOCTL_TC_GET:
    552         {
    553                 unsigned int *tc = argp;
    554                 router_tc_read(priv, tc);
    555                 break;
    556         }
    557 
    558         default: return RTEMS_NOT_IMPLEMENTED;
    559         }
    560 
    561         return 0;
    562 }
     1832
     1833        return ROUTER_ERR_OK;
     1834}
     1835
     1836int router_tc_enable(void *d)
     1837{
     1838        struct router_priv *priv = d;
     1839        int error = router_check_open(d);
     1840
     1841        if (error)
     1842                return error;
     1843
     1844        REG_WRITE(&priv->regs->timecode, TC_EN);
     1845
     1846        return ROUTER_ERR_OK;
     1847}
     1848
     1849int router_tc_disable(void *d)
     1850{
     1851        struct router_priv *priv = d;
     1852        int error = router_check_open(d);
     1853
     1854        if (error)
     1855                return error;
     1856
     1857        REG_WRITE(&priv->regs->timecode, 0);
     1858
     1859        return ROUTER_ERR_OK;
     1860}
     1861
     1862int router_tc_reset(void *d)
     1863{
     1864        struct router_priv *priv = d;
     1865        unsigned int tc;
     1866        int error = router_check_open(d);
     1867
     1868        if (error)
     1869                return error;
     1870
     1871        tc = REG_READ(&priv->regs->timecode);
     1872        REG_WRITE(&priv->regs->timecode, tc | TC_RE);
     1873
     1874        return ROUTER_ERR_OK;
     1875}
     1876
     1877int router_tc_get(void *d)
     1878{
     1879        struct router_priv *priv = d;
     1880        int error = router_check_open(d);
     1881
     1882        if (error)
     1883                return error;
     1884
     1885        return (REG_READ(&priv->regs->timecode) & (TC_CF | TC_TC)) >> TC_TC_BIT;
     1886}
     1887
     1888int router_interrupt_unmask(void *d, int options)
     1889{
     1890        struct router_priv *priv = d;
     1891        unsigned int mask;
     1892        int error = router_check_open(d);
     1893
     1894        if (error)
     1895                return error;
     1896
     1897        #ifdef THREAD_SAFE
     1898        /* Take device lock - Wait until we get semaphore */
     1899        if (rtems_semaphore_obtain(priv->sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
     1900                        != RTEMS_SUCCESSFUL) {
     1901                DBG("ROUTER Sempahore failed\n");
     1902                return ROUTER_ERR_ERROR;
     1903        }
     1904        #endif
     1905
     1906        /* Unmask interrupts in ROTUER */
     1907        /* Get previous mask */
     1908        mask = REG_READ(&priv->regs->imask);
     1909
     1910        /* Clear previous interrupts*/
     1911        REG_WRITE(&priv->regs->pip, 0xffffffff);
     1912
     1913        /* Set new mask */
     1914        REG_WRITE(&priv->regs->imask, mask | options);
     1915
     1916        #ifdef THREAD_SAFE
     1917        /* Unlock dev */
     1918        rtems_semaphore_release(priv->sem);
     1919        #endif
     1920
     1921        return ROUTER_ERR_OK;
     1922}
     1923
     1924int router_interrupt_mask(void *d, int options)
     1925{
     1926        struct router_priv *priv = d;
     1927        unsigned int mask;
     1928        int error = router_check_open(d);
     1929
     1930        if (error)
     1931                return error;
     1932
     1933        #ifdef THREAD_SAFE
     1934        /* Take device lock - Wait until we get semaphore */
     1935        if (rtems_semaphore_obtain(priv->sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
     1936                        != RTEMS_SUCCESSFUL) {
     1937                DBG("ROUTER Sempahore failed\n");
     1938                return ROUTER_ERR_ERROR;
     1939        }
     1940        #endif
     1941
     1942        /* Mask interrupts in ROTUER */
     1943        /* Get previous mask */
     1944        mask = REG_READ(&priv->regs->imask);
     1945
     1946        /* Clear previous interrupts*/
     1947        REG_WRITE(&priv->regs->pip, 0xffffffff);
     1948
     1949        /* Set new mask */
     1950        REG_WRITE(&priv->regs->imask, mask & ~(options));
     1951
     1952        #ifdef THREAD_SAFE
     1953        /* Unlock dev */
     1954        rtems_semaphore_release(priv->sem);
     1955        #endif
     1956
     1957        return ROUTER_ERR_OK;
     1958}
     1959
     1960int router_port_interrupt_unmask(void *d, int port)
     1961{
     1962        struct router_priv *priv = d;
     1963        unsigned int mask;
     1964        int error = router_check_port(d, port);
     1965
     1966        if (error)
     1967                return error;
     1968
     1969        #ifdef THREAD_SAFE
     1970        /* Take device lock - Wait until we get semaphore */
     1971        if (rtems_semaphore_obtain(priv->sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
     1972                        != RTEMS_SUCCESSFUL) {
     1973                DBG("ROUTER Sempahore failed\n");
     1974                return ROUTER_ERR_ERROR;
     1975        }
     1976        #endif
     1977
     1978        /* Unmask interrupts in ROTUER */
     1979        /* Get previous mask */
     1980        mask = REG_READ(&priv->regs->ipmask);
     1981
     1982        /* Clear previous interrupts*/
     1983        REG_WRITE(&priv->regs->pip, (0x1 << port));
     1984
     1985        /* Set new mask */
     1986        REG_WRITE(&priv->regs->ipmask, mask | (0x1 << port));
     1987
     1988        #ifdef THREAD_SAFE
     1989        /* Unlock dev */
     1990        rtems_semaphore_release(priv->sem);
     1991        #endif
     1992
     1993        return ROUTER_ERR_OK;
     1994}
     1995
     1996int router_port_interrupt_mask(void *d, int port)
     1997{
     1998        struct router_priv *priv = d;
     1999        unsigned int mask;
     2000        int error = router_check_port(d, port);
     2001
     2002        if (error)
     2003                return error;
     2004
     2005        #ifdef THREAD_SAFE
     2006        /* Take device lock - Wait until we get semaphore */
     2007        if (rtems_semaphore_obtain(priv->sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
     2008                        != RTEMS_SUCCESSFUL) {
     2009                DBG("ROUTER Sempahore failed\n");
     2010                return ROUTER_ERR_ERROR;
     2011        }
     2012        #endif
     2013
     2014        /* Mask interrupts in ROTUER */
     2015        /* Get previous mask */
     2016        mask = REG_READ(&priv->regs->ipmask);
     2017
     2018        /* Clear previous interrupts*/
     2019        REG_WRITE(&priv->regs->pip, (0x1 << port));
     2020
     2021        /* Set new mask */
     2022        REG_WRITE(&priv->regs->ipmask, mask & ~(0x1 << port));
     2023
     2024        #ifdef THREAD_SAFE
     2025        /* Unlock dev */
     2026        rtems_semaphore_release(priv->sem);
     2027        #endif
     2028
     2029        return ROUTER_ERR_OK;
     2030}
     2031
     2032int router_reset(void *d)
     2033{
     2034        struct router_priv *priv = d;
     2035        int error = router_check_open(d);
     2036
     2037        if (error)
     2038                return error;
     2039
     2040        /* Reset router */
     2041        REG_WRITE(&priv->regs->cfgsts, RTRCFG_RE);
     2042
     2043        return ROUTER_ERR_OK;
     2044}
     2045
     2046int router_icodegen_enable(void *d, uint8_t intn, uint32_t aitimer,
     2047                int options)
     2048{
     2049        struct router_priv *priv = d;
     2050        int error = router_check_distint_support(d);
     2051
     2052        if (error)
     2053                return error;
     2054
     2055        #ifdef THREAD_SAFE
     2056        /* Take device lock - Wait until we get semaphore */
     2057        if (rtems_semaphore_obtain(priv->sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT)
     2058                        != RTEMS_SUCCESSFUL) {
     2059                DBG("ROUTER Sempahore failed\n");
     2060                return ROUTER_ERR_ERROR;
     2061        }
     2062        #endif
     2063
     2064        REG_WRITE(&priv->regs->icodegen, (options & ~(ICODEGEN_IN)) |
     2065                        ICODEGEN_EN | (intn & ICODEGEN_IN));
     2066
     2067        if (options & ICODEGEN_TE) {
     2068                REG_WRITE(&priv->regs->aitimer, (aitimer & AITIMER_RL));
     2069        }
     2070
     2071        #ifdef THREAD_SAFE
     2072        /* Unlock dev */
     2073        rtems_semaphore_release(priv->sem);
     2074        #endif
     2075
     2076        return ROUTER_ERR_OK;
     2077}
     2078
     2079int router_icodegen_disable(void *d)
     2080{
     2081        struct router_priv *priv = d;
     2082        int error = router_check_distint_support(d);
     2083
     2084        if (error)
     2085                return error;
     2086
     2087        REG_WRITE(&priv->regs->icodegen, ICODEGEN_TE);
     2088
     2089        return ROUTER_ERR_OK;
     2090}
     2091
     2092int router_isrctimer_set(void *d, uint32_t reloadvalue)
     2093{
     2094        struct router_priv *priv = d;
     2095        int error = router_check_distint_support(d);
     2096
     2097        if (error)
     2098                return error;
     2099
     2100        /* Set ISRC TIMER */
     2101        REG_WRITE(&priv->regs->isrctimer, (reloadvalue & (ISRCTIMER_RL)));
     2102
     2103        return ROUTER_ERR_OK;
     2104}
     2105
     2106int router_isrtimer_set(void *d, uint32_t reloadvalue)
     2107{
     2108        struct router_priv *priv = d;
     2109        int error = router_check_distint_support(d);
     2110
     2111        if (error)
     2112                return error;
     2113
     2114        /* Set ISR TIMER */
     2115        REG_WRITE(&priv->regs->isrtimer, (reloadvalue & (ISRTIMER_RL)));
     2116
     2117        return ROUTER_ERR_OK;
     2118}
     2119
     2120int router_isrctimer_get(void *d, uint32_t *reloadvalue)
     2121{
     2122        struct router_priv *priv = d;
     2123        int error = router_check_distint_support(d);
     2124
     2125        if (error)
     2126                return error;
     2127
     2128        if (reloadvalue == NULL) {
     2129                DBG("ROUTER Wrong reloadvalue pointer\n");
     2130                return ROUTER_ERR_EINVAL;
     2131        }
     2132
     2133        /* Set ISRC TIMER */
     2134        *reloadvalue = REG_READ(&priv->regs->isrctimer) & (ISRCTIMER_RL);
     2135
     2136        return ROUTER_ERR_OK;
     2137}
     2138
     2139int router_isrtimer_get(void *d, uint32_t *reloadvalue)
     2140{
     2141        struct router_priv *priv = d;
     2142        int error = router_check_distint_support(d);
     2143
     2144        if (error)
     2145                return error;
     2146
     2147        if (reloadvalue == NULL) {
     2148                DBG("ROUTER Wrong reloadvalue pointer\n");
     2149                return ROUTER_ERR_EINVAL;
     2150        }
     2151
     2152        /* Set ISR TIMER */
     2153        *reloadvalue = REG_READ(&priv->regs->isrtimer) & (ISRTIMER_RL);
     2154
     2155        return ROUTER_ERR_OK;
     2156}
Note: See TracChangeset for help on using the changeset viewer.