source: rtems/c/src/lib/libbsp/sparc/shared/spw/grspw_router.c @ 59af2cc

5
Last change on this file since 59af2cc was 59af2cc, checked in by Javier Jalle <javier.jalle@…>, on 03/07/17 at 12:59:09

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.
  • Property mode set to 100644
File size: 48.2 KB
Line 
1/* GRSPW ROUTER APB-Register Driver.
2 *
3 * COPYRIGHT (c) 2010-2017.
4 * Cobham Gaisler AB.
5 *
6 * The license and distribution terms for this file may be
7 * found in the file LICENSE in this distribution or at
8 * http://www.rtems.org/license/LICENSE.
9 */
10
11#include <rtems.h>
12#include <rtems/libio.h>
13#include <rtems/bspIo.h>
14#include <stdio.h>
15#include <bsp.h>
16#include <rtems/bspIo.h> /* printk */
17
18#include <drvmgr/drvmgr.h>
19#include <drvmgr/ambapp_bus.h>
20#include <bsp/grspw_router.h>
21
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
506
507struct router_regs {
508        unsigned int resv1;             /* 0x000 */
509        unsigned int psetup[255];       /* 0x004 */
510        unsigned int resv2;             /* 0x400 */
511        unsigned int routes[255];       /* 0x404 */
512        unsigned int pctrl[32];         /* 0x800 */
513        unsigned int psts[32];          /* 0x880 */
514        unsigned int treload[32];       /* 0x900 */
515        unsigned int pctrl2[32];        /* 0x980 */
516        unsigned int cfgsts;            /* 0xA00 */
517        unsigned int timecode;          /* 0xA04 */
518        unsigned int ver;               /* 0xA08 */
519        unsigned int idiv;              /* 0xA0C */
520        unsigned int cfgwe;             /* 0xA10 */
521        unsigned int tprescaler;        /* 0xA14 */
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 */
537        unsigned int chari[31];         /* 0xC84 */
538        unsigned int resv7;             /* 0xD00 */
539        unsigned int pkto[31];          /* 0xD04 */
540        unsigned int resv8;             /* 0xD80 */
541        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 */
548};
549
550struct router_priv {
551        struct drvmgr_dev *dev;
552
553        /* ROUTER control registers */
554        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
565        int minor;
566        int open;
567        struct router_hw_info hwinfo;
568        int nports;
569        int irq_init;
570
571        SPIN_DECLARE(plock[32])
572
573};
574
575int router_count = 0;
576static struct router_priv *priv_tab[ROUTER_MAX];
577
578/* Driver prototypes */
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);
594
595int router_init2(struct drvmgr_dev *dev);
596
597struct drvmgr_drv_ops router_ops =
598{
599        .init = {NULL,  router_init2, NULL, NULL},
600        .remove = NULL,
601        .info = NULL
602};
603
604struct amba_dev_id router_ids[] =
605{
606        {VENDOR_GAISLER, GAISLER_SPW_ROUTER},
607        {0, 0}          /* Mark end of table */
608};
609
610struct amba_drv_info router_drv_info =
611{
612        {
613                DRVMGR_OBJ_DRV,                 /* Driver */
614                NULL,                           /* Next driver */
615                NULL,                           /* Device list */
616                DRIVER_AMBAPP_GAISLER_SPW_ROUTER_ID,/* Driver ID */
617                "ROUTER_DRV",                   /* Driver Name */
618                DRVMGR_BUS_TYPE_AMBAPP,         /* Bus Type */
619                &router_ops,
620                NULL,                           /* Funcs */
621                0,                              /* No devices yet */
622                sizeof(struct router_priv),     /* Let DRVMGR allocate for us */
623        },
624        &router_ids[0],
625};
626
627void router_register_drv (void)
628{
629        DBG("Registering SPW ROUTER driver\n");
630        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;
781}
782
783int router_init2(struct drvmgr_dev *dev)
784{
785        struct router_priv *priv = dev->priv;
786        struct amba_dev_info *ambadev;
787        struct ambapp_core *pnpinfo;
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) {
799                return DRVMGR_NOMEM;
800        }
801        priv->dev = dev;
802
803        /* Get device information from AMBA PnP information */
804        ambadev = (struct amba_dev_info *)priv->dev->businfo;
805        if (ambadev == NULL) {
806                return DRVMGR_FAIL;
807        }
808        pnpinfo = &ambadev->info;
809        priv->regs = (struct router_regs *)pnpinfo->ahb_slv->start[0];
810        priv->minor = dev->minor_drv;
811
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;
879        } else {
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;
914        } else {
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;
954                }
955        }
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;
1188        int i;
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
1813        REG_WRITE(&priv->regs->cfgsts, cfgsts);
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
1831        *cfgsts = REG_READ(&priv->regs->cfgsts);
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 TracBrowser for help on using the repository browser.