1: /*
2: *----------------------------------------------------------------------
3: * T2EX Software Package
4: *
5: * Copyright 2012 by Ken Sakamura.
6: * This software is distributed under the latest version of T-License 2.x.
7: *----------------------------------------------------------------------
8: *
9: * Released by T-Engine Forum(http://www.t-engine.org/) at 2012/12/12.
10: * Modified by T-Engine Forum at 2013/07/14.
11: * Modified by TRON Forum(http://www.tron.org/) at 2015/06/04.
12: *
13: *----------------------------------------------------------------------
14: */
15: /*
16: * This software package is available for use, modification,
17: * and redistribution in accordance with the terms of the attached
18: * T-License 2.x.
19: * If you want to redistribute the source code, you need to attach
20: * the T-License 2.x document.
21: * There's no obligation to publish the content, and no obligation
22: * to disclose it to the TRON Forum if you have modified the
23: * software package.
24: * You can also distribute the modified source code. In this case,
25: * please register the modification to T-Kernel traceability service.
26: * People can know the history of modifications by the service,
27: * and can be sure that the version you have inherited some
28: * modification of a particular version or not.
29: *
30: * http://trace.tron.org/tk/?lang=en
31: * http://trace.tron.org/tk/?lang=ja
32: *
33: * As per the provisions of the T-License 2.x, TRON Forum ensures that
34: * the portion of the software that is copyrighted by Ken Sakamura or
35: * the TRON Forum does not infringe the copyrights of a third party.
36: * However, it does not make any warranty other than this.
37: * DISCLAIMER: TRON Forum and Ken Sakamura shall not be held
38: * responsible for any consequences or damages caused directly or
39: * indirectly by the use of this software package.
40: *
41: * The source codes in bsd_source.tar.gz in this software package are
42: * derived from NetBSD or OpenBSD and not covered under T-License 2.x.
43: * They need to be changed or redistributed according to the
44: * representation of each source header.
45: */
46:
47: /*
48: * cpu_support.S
49: *
50: * CPU operation specific to EM1-D512, etc.
51: */
52: #define _in_asm_source_
53:
54: #include <machine.h>
55: #include <tk/errno.h>
56: #include <tk/sysdef.h>
57: #include <tk/asm.h>
58: #include <sys/sysinfo.h>
59:
60: #include "config.h"
61: #include "cpu_conf.h"
62: #include "isysconf.h"
63: #include "tkdev_conf.h"
64: #include "offset.h"
65:
66: /* ------------------------------------------------------------------------ */
67: /*
68: * Dispatcher
69: * dispatch_to_schedtsk:
70: * Discard current context and, dispatch to schedtsk forcibly.
71: * Jumping to it directly using (bx) and never returns.
72: * Called when stack is not defined (ssp is undefined).
73: * Called when interrupt is disabled.
74: * dispatch_entry:
75: * Ordinary dispatch processing. Called by svc, SWI_DISPATCH.
76: * _ret_int_dispatch:
77: * Called if tk_ret_int() requires dispatching.
78: *
79: * Saved context
80: * Save registers except for ssp(R13_svc) to stack. ssp is saved in TCB.
81: *
82: * +---------------+
83: * ssp -> | R0--R11 |
84: * | taskmode |
85: * | R13_usr = usp | valid only in RNG1-3
86: * | R14_usr |
87: * +---------------+
88: * | R14_svc | R14_svc before interrupt
89: * | | (valid only in case of tk_ret_int)
90: * +---------------+
91: * | R12 = ip | ^ saved by interrupt entry
92: * | R14_svc = lr | return address(pc) | routine
93: * | SPSR_svc | V
94: * +---------------+
95: */
96:
97: /* temporary stack used when dispatch_to_schedtsk is called
98: */
99: #define TMP_STACK_SZ (4*1024)
100: #define TMP_STACK_TOP (tmp_stack + TMP_STACK_SZ)
101: .lcomm tmp_stack, TMP_STACK_SZ
102:
103: .text
104: .balign 4
105: .globl Csym(dispatch_to_schedtsk)
106: .type Csym(dispatch_to_schedtsk), %function
107: .globl Csym(dispatch_entry)
108: .type Csym(dispatch_entry), %function
109:
110: Csym(dispatch_to_schedtsk):
111: /* SVC mode / interrupt-disabled state CPSR.I=1 A=1 */
112: ldr sp, =TMP_STACK_TOP // set up a temporary stack
113:
114: ldr ip, =Csym(dispatch_disabled)
115: ldr r0, =1
116: str r0, [ip] // disable dispatch
117:
118: ldr r4, =Csym(ctxtsk) // R4 = &ctxtsk
119: ldr ip, =TASKMODE
120: ldr r0, =0
121: #if USE_DBGSPT
122: ldr r8, [r4]
123: #endif
124: str r0, [r4] // ctxtsk = NULL
125: str r0, [ip] // taskmode = 0
126:
127: cpsie aif // enable interrupt
128: b l_dispatch0
129:
130: Csym(dispatch_entry):
131: /* SVC mode / interrupt-disabled state CPSR.I=1 A=1 */
132: ldr ip, [sp, #2*4]
133: bic ip, ip, #PSR_DI
134: str ip, [sp, #2*4] // adjust SPSR_svc
135:
136: stmfd sp!, {lr} // save context (R14_svc)
137: // meaningless place holder for proper stack alignment
138: _ret_int_dispatch:
139: /* SVC mode / interrupt-disabled state CPSR.I=1 A=1 */
140: ldr ip, =Csym(dispatch_disabled)
141: ldr lr, =1
142: str lr, [ip] // disable dispatch
143:
144: cpsie aif // enable interrupt
145:
146: ldr ip, =TASKMODE
147: ldr ip, [ip]
148: sub sp, sp, #15*4
149: stmia sp, {r0-r11, ip, sp, lr}^ // save context
150:
151: ldr r4, =Csym(ctxtsk) // R4 = &ctxtsk
152: ldr ip, =TASKMODE
153: ldr r0, =0
154: ldr r8, [r4]
155: str sp, [r8, #TCB_tskctxb + CTXB_ssp] // save ssp to TCB
156: str r0, [r4] // ctxtsk = NULL
157: str r0, [ip] // taskmode = 0
158:
159: l_dispatch0:
160: bic sp, sp, #7 // align stack module 8 bytes
161:
162: /* interrupt-enabled state CPSR.I=0 A=0 */
163: #if USE_DBGSPT
164: ldr ip, =hook_stop_jmp // hook processing
165: ldr pc, [ip]
166: ret_hook_stop:
167: #endif
168:
169: ldr r5, =Csym(schedtsk) // R5 = &schedtsk
170: ldr r6, =Csym(lowpow_discnt) // R6 = &lowpow_discnt
171:
172: l_dispatch1:
173: cpsid IMASK // disable interrupt
174:
175: ldr r8, [r5] // R8 = schedtsk
176: cmp r8, #0 // is there schedtsk ?
177: bne l_dispatch2
178:
179: /* enter low-power mode since there is no task to execute */
180: ldr ip, [r6] // is low_pow disabled?
181: cmp ip, #0
182: bleq Csym(low_pow) // call low_pow()
183:
184: cpsie aif // enable interrupt
185: b l_dispatch1
186:
187: l_dispatch2: // dispatch to schedtsk
188: /* interrupt-disabled state CPSR.I=1 A=1 */
189: str r8, [r4] // ctxtsk = schedtsk
190: ldr sp, [r8, #TCB_tskctxb + CTXB_ssp] // restore ssp from TCB
191:
192: /* switch task spaces */
193: ldr r0, =0
194: ldr r1, [r8, #TCB_tskctxb + CTXB_uatb]
195: ldr r2, [r8, #TCB_tskctxb + CTXB_lsid]
196: mrc p15, 0, ip, cr2, c0, 1 // TTBR1
197: cmp r1, #0
198: andne ip, ip, #0x7f
199: orrne ip, ip, r1
200: mcr p15, 0, r0, cr13, c0, 1 // set ASID to a meaningless value
201: .ISB r0, r0 // for synchronizing ASID and TTBR
202: mcr p15, 0, ip, cr2, c0, 0 // TTBR0
203: mcr p15, 0, r2, cr13, c0, 1 // CONTEXTIDR
204: .ISB r0, r0
205:
206: #if USE_DBGSPT
207: ldr ip, =hook_exec_jmp // hook processing
208: ldr pc, [ip]
209: ret_hook_exec:
210: #endif
211:
212: ldr ip, =Csym(dispatch_disabled)
213: ldr lr, =0
214: str lr, [ip] // enable dispatch
215:
216: clrex // clear exclusive lock state
217: // it is required for atomic
218: // operation using LDREX/STREX
219:
220: ldr ip, [r8, #TCB_reqdct] // request DCT
221: cmp ip, #1
222:
223: ldmia sp, {r0-r11, ip, sp, lr}^ // restore context
224: nop
225: add sp, sp, #15*4
226: ldr lr, =TASKMODE
227: str ip, [lr]
228: movne ip, #0 // ip = 0 when no DCT request
229:
230: ldmfd sp!, {lr} // restore R14_svc
231:
232: ands ip, ip, #TMF_CPL(3) // DCT is ineffective in protection level 0
233: bne dct_startup // jump to DCT processing
234:
235: EXC_RETURN
236:
237:
238: #if USE_DBGSPT
239: /*
240: * Hook routine invocation in task dispatcher
241: * void stop( ID tskid, INT lsid, UINT tskstat )
242: * void exec( ID tskid, INT lsid )
243: */
244: .text
245: .balign 4
246: hook_stop:
247: cmp r8, #0 // r8 = ctxtsk
248: beq l_notask
249:
250: ldrb r2, [r8, #TCB_state] // tskstat
251: mov r2, r2, lsl #1
252: ldr r1, [r8, #TCB_tskctxb + CTXB_lsid] // lsid
253: ldr r0, [r8, #TCB_tskid] // tskid
254:
255: ldr ip, =Csym(hook_stopfn)
256: ldr ip, [ip]
257: blx ip // call stop(tskid, lsid, tskstat)
258:
259: l_notask:
260: b ret_hook_stop
261:
262: hook_exec:
263: mov r9, sp // save sp
264: bic sp, sp, #7 // align stack module 8 bytes
265:
266: // r8 = ctxtsk
267: ldr r1, [r8, #TCB_tskctxb + CTXB_lsid] // lsid
268: ldr r0, [r8, #TCB_tskid] // tskid
269:
270: ldr ip, =Csym(hook_execfn)
271: ldr ip, [ip]
272: blx ip // call exec(tskid, lsid)
273:
274: mov sp, r9 // restore sp
275: b ret_hook_exec
276:
277: /*
278: * Setting and releasing of task dispatcher hook routine
279: */
280: .text
281: .balign 4
282: .globl Csym(hook_dsp)
283: .type Csym(hook_dsp), %function
284: Csym(hook_dsp):
285: ldr r0, =hook_exec_jmp
286: ldr r1, =hook_stop_jmp
287: ldr r2, =hook_exec
288: ldr r3, =hook_stop
289: str r2, [r0]
290: str r3, [r1]
291: bx lr
292:
293: .globl Csym(unhook_dsp)
294: .type Csym(unhook_dsp), %function
295: Csym(unhook_dsp):
296: ldr r0, =hook_exec_jmp
297: ldr r1, =hook_stop_jmp
298: ldr r2, =ret_hook_exec
299: ldr r3, =ret_hook_stop
300: str r2, [r0]
301: str r3, [r1]
302: bx lr
303:
304: .data
305: .balign 4
306: hook_exec_jmp: .long ret_hook_exec
307: hook_stop_jmp: .long ret_hook_stop
308:
309: #endif /* USE_DBGSPT */
310:
311: /* ------------------------------------------------------------------------ */
312: /*
313: * High-level language support routine for interrupt handler
314: * this is called from interrupt entry routine with information
315: * in the stack as follows.
316: * +---------------+ SVC stack
317: * ssp -> | R13_svc = ssp |
318: * | R12_usr = ip |
319: * | (padding) | stack alignment (if necessary)
320: * | CPSR_xxx |
321: * | R14_svc = lr |
322: * +---------------+
323: *
324: * +---------------+ exception stack
325: * | R0 - R2 |
326: * +===============+
327: * isp -> | R3 | only in the case of FIQ and IRQ
328: * +---------------+
329: * isp -> | R12 = ip | R12_usr or R12_fiq
330: * | R14_xxx = lr |
331: * | SPSR |
332: * +---------------+
333: *
334: * ip holds the vector table address
335: * (ip - EIT_VECTBL) / 4 = vector number
336: */
337: .text
338: .balign 4
339: .globl Csym(defaulthdr_startup)
340: .type Csym(defaulthdr_startup), %function
341: .globl Csym(inthdr_startup)
342: .type Csym(inthdr_startup), %function
343: .globl Csym(exchdr_startup)
344: .type Csym(exchdr_startup), %function
345: Csym(defaulthdr_startup):
346: /* unknown mode / interrupt-disabled state CPSR.I=1 A=? F=? */
347: mrs lr, cpsr
348: and lr, lr, #PSR_M(31)
349: cmp lr, #PSR_FIQ
350: cmpne lr, #PSR_IRQ
351: stmnefd sp!, {r3} // save registers
352: stmfd sp!, {r0-r2}
353:
354: ldr r3, =EIT_VECTBL
355: sub r3, ip, r3
356: // argument to the handler
357: mov r0, r3, lsr #2 // r0 = dintno
358: add r1, sp, #4*4 // r1 = sp
359:
360: ldr r3, =EIT_DEFAULT * 4 // r3 = vector table offset
361: b l_inthdr2
362:
363: Csym(exchdr_startup):
364: /* unknown mode / interrupt-disabled state CPSR.I=1 A=? F=? */
365: stmfd sp!, {r3} // save registers
366: b l_inthdr1
367:
368: Csym(inthdr_startup):
369: /* unknown mode / interrupt-disabled state CPSR.I=1 A=1 F=? */
370: mrs lr, cpsr
371: and lr, lr, #PSR_M(31)
372: cmp lr, #PSR_SVC
373: stmeqfd sp!, {r3} // save r3 in the case of SVC
374: l_inthdr1:
375: stmfd sp!, {r0-r2} // save registers
376:
377: ldr r3, =EIT_VECTBL
378: sub r3, ip, r3 // r3 = vector table offset
379:
380: // argument to the handler
381: mov r0, r3, lsr #2 // r0 = dintno
382: add r1, sp, #4*4 // r1 = sp
383:
384: l_inthdr2:
385: mrs r2, cpsr // save CPSR to r2
386: cpsid IMASK, #PSR_SVC // enter SVC mode, interrupt is disabled
387:
388: stmfd sp!, {r2, lr} // save CPSR and lr_svc
389:
390: mov r2, sp
391: bic sp, sp, #7 // align stack module 8 bytes
392: stmfd sp!, {r2, ip} // save sp and ip
393:
394: ldr ip, =TASKINDP // enter task independent portion
395: ldr lr, [ip]
396: add lr, lr, #1
397: str lr, [ip]
398:
399: #if USE_DBGSPT
400: ldr ip, =hook_ienter_jmp
401: ldr pc, [ip]
402: ret_hook_ienter:
403: #endif
404:
405: ldr ip, =Csym(hll_inthdr)
406: ldr ip, [ip, r3]
407: blx ip // call hll_inthdr[n](dintno, sp)
408:
409: #if USE_DBGSPT
410: ldr ip, =hook_ileave_jmp
411: ldr pc, [ip]
412: ret_hook_ileave:
413: #endif
414:
415: ldmfd sp!, {r2, ip} // restore sp and ip
416: mov sp, r2
417: ldmfd sp!, {r2, r3} // r2 contaisn the original mode
418: // r3 = R14_svc
419: orr r2, r2, #PSR_DI
420: msr cpsr_xc, r2 // restore original mode, interrupt is disabled
421:
422: ldr ip, =TASKINDP
423: ldr lr, [ip]
424: sub lr, lr, #1
425: str lr, [ip]
426:
427: ldmfd sp!, {r0-r2} // restore registers
428: swp r3, r3, [sp] // restore r3, R14_svc is saved
429: swi SWI_RETINT // tk_ret_int()
430:
431:
432: #if USE_DBGSPT
433: /*
434: * Invoking a hook routine of an interrupt handler
435: */
436: .text
437: .balign 4
438: hook_ienter:
439: stmfd sp!, {r0-r1} // save registers
440: stmfd sp!, {r2-r3} // align stack module 8
441:
442: ldr ip, =Csym(hook_ienterfn)
443: ldr ip, [ip]
444: blx ip // call enter(dintno, sp)
445:
446: ldmfd sp!, {r2-r3} // restore registers
447: ldmfd sp, {r0-r1} // leave dintno and sp on the stack
448: b ret_hook_ienter
449:
450: hook_ileave:
451: ldmfd sp!, {r0-r1} // restore dintno and sp
452:
453: ldr ip, =Csym(hook_ileavefn)
454: ldr ip, [ip]
455: blx ip // call leave(dintno, sp)
456:
457: b ret_hook_ileave
458:
459: /*
460: * Setting and relaseing hook routine of an interrupt handler
461: */
462: .text
463: .balign 4
464: .globl Csym(hook_int)
465: .type Csym(hook_int), %function
466: Csym(hook_int):
467: ldr r0, =hook_ienter_jmp
468: ldr r1, =hook_ileave_jmp
469: ldr r2, =hook_ienter
470: ldr r3, =hook_ileave
471: str r2, [r0]
472: str r3, [r1]
473: bx lr
474:
475: .globl Csym(unhook_int)
476: .type Csym(unhook_int), %function
477: Csym(unhook_int):
478: ldr r0, =hook_ienter_jmp
479: ldr r1, =hook_ileave_jmp
480: ldr r2, =ret_hook_ienter
481: ldr r3, =ret_hook_ileave
482: str r2, [r0]
483: str r3, [r1]
484: bx lr
485:
486: .data
487: .balign 4
488: hook_ienter_jmp: .long ret_hook_ienter
489: hook_ileave_jmp: .long ret_hook_ileave
490:
491: #endif /* USE_DBGSPT */
492:
493: /*
494: * Processing tk_ret_int()
495: * interrupt stack looks as follows when this is called.
496: * +---------------+
497: * ssp -> | R12_usr | saved by svc SWI_RETINT
498: * | R14_svc |
499: * | SPSR_svc |
500: * +---------------+
501: *
502: * +---------------+
503: * isp -> | R14_svc | saved when tk_ret_int was called
504: * +---------------+
505: * | R12_xxx | saved at the time of interrupt
506: * | R14_xxx | <- return address
507: * | SPSR_xxx |
508: * +---------------+
509: */
510: .text
511: .balign 4
512: .globl Csym(_tk_ret_int)
513: .type Csym(_tk_ret_int), %function
514: Csym(_tk_ret_int):
515: ldr ip, [sp, #2*4] // ip = SPSR
516:
517: and lr, ip, #PSR_M(31)
518: cmp lr, #PSR_SVC
519: beq l_retint_svc // is this tk_ret_int invoked from an SVC?
520:
521: stmfd sp!, {r2, r3} // r2 is used as temporary work register (r3 is saved as place holder)
522: add r2, sp, #4
523:
524: orr ip, ip, #PSR_DI
525: bic ip, ip, #PSR_T
526: cmp lr, #PSR_FIQ
527: msr cpsr_xc, ip // enter interrupted mode, interrupt is disabled
528:
529: ldr ip, [sp, #0*4] // copy isp to ssp
530: ldr lr, [sp, #3*4]
531: str ip, [r2, #0*4] // R14_svc
532: str lr, [r2, #3*4] // SPSR_xxx
533: ldr ip, [sp, #1*4]
534: ldr lr, [sp, #2*4]
535: strne ip, [r2, #1*4] // R12_xxx (in non-FIQ cases only)
536: str lr, [r2, #2*4] // R14_xxx (return address)
537: add sp, sp, #4*4 // pop isp
538:
539: cpsid IMASK, #PSR_SVC // enter SVC mode, interrupt is disabled
540:
541: ldmfd sp!, {r2} // restore r2
542: b l_retint1
543:
544: l_retint_svc:
545: add sp, sp, #3*4 // discard the value saved by svc SWI_RETINT
546:
547: cpsid IMASK, #PSR_SVC // disable interrupt
548:
549: l_retint1:
550: ldr ip, =TASKINDP // is there additional multi-interrupt(s)?
551: ldr ip, [ip]
552: cmp ip, #0
553: bne l_nodispatch
554:
555: ldr ip, =Csym(dispatch_disabled) // is dispatch disabled?
556: ldr ip, [ip]
557: cmp ip, #0
558: bne l_nodispatch
559:
560: ldr ip, [sp, #3*4] // SPSR
561: tst ip, #PSR_A|PSR_I|PSR_F // is this exception during interrupt-disabled state?
562: bne l_nodispatch
563:
564: ldr ip, =Csym(ctxtsk) // is dispatch necessary?
565: ldr lr, =Csym(schedtsk)
566: ldr ip, [ip]
567: ldr lr, [lr]
568: cmp ip, lr
569: bne _ret_int_dispatch // goto dispatch processing
570:
571: l_nodispatch:
572: ldmfd sp!, {lr} // restore lr
573: EXC_RETURN
574:
575:
576: /* ------------------------------------------------------------------------ */
577:
578: /*
579: * Unsupported system call
580: */
581: .text
582: .balign 4
583: .globl Csym(no_support)
584: .type Csym(no_support), %function
585: Csym(no_support):
586: ldr r0, =E_RSFN
587: bx lr
588:
589: /*
590: * System call entry table
591: */
592: .text
593: .balign 4
594: _svctbl:
595: .int Csym(no_support)
596: #define _tk_ret_int no_support
597: #include <sys/svc/tksvctbl.h>
598: #undef _tk_ret_int
599:
600: /*
601: * System call entry
602: * no need to save temporary registers
603: * permanent registers are saved by the code generated by the compiler
604: * +---------------+
605: * | exinf | save for hook
606: * +---------------+
607: * | svc_ssp | save within call_entry
608: * | taskmode |
609: * | r9 |
610: * | r10 |
611: * | fp |
612: * +===============+
613: * ssp -> | ip | function code
614: * | lr | return address
615: * | SPSR |
616: * +---------------+
617: *
618: * Save ssp to CTXB.svc_ssp when a T-Kernel system call (SVC) is invoked.
619: * However, if this is a nested SVC, svc_ssp is not updated.
620: * Save the original svc_ssp to the system stack.
621: * svc_ssp is restored when the system call returns.
622: * svc_ssp = NULL means that SVC is not being callled.
623: * With extended SVC, svc_ssp = NULL holds.
624: */
625: .text
626: .balign 4
627: .globl Csym(call_entry)
628: .type Csym(call_entry), %function
629: Csym(call_entry):
630: /* SVC mode / interrupt-disabled state CPSR.I=1 A=? F=? */
631: ldr ip, [sp, #2*4]
632: and ip, ip, #PSR_I|PSR_F // cpsr_c doesn't include PSR_A
633: orr ip, ip, #PSR_SVC
634: msr cpsr_c, ip // restore interrupt disabled status to the original.
635:
636: stmfd sp!, {r9-r10, fp} // save working registers
637: add fp, sp, #3*4 // fp is the sp when call_entry was entered.
638:
639: ldr ip, =TASKMODE // obtain taskmode flag
640: ldr r10, [ip] // r10 = taskmode
641: stmfd sp!, {r10} // save taskmode
642: mov lr, r10, lsl #16 // update taskmode flag
643: str lr, [ip]
644:
645: ldr r9, =Csym(ctxtsk) // r9 = ctxtsk
646: ldr r9, [r9] // if no ctxtsk (NULL) then
647: cmp r9, #0 // assign a dumm value
648: ldreq r9, =dummy_svc_ssp - (TCB_tskctxb + CTXB_svc_ssp)
649: ldr ip, [r9, #TCB_tskctxb + CTXB_svc_ssp]
650: cmp ip, #0
651: streq fp, [r9, #TCB_tskctxb + CTXB_svc_ssp] // update svc_ssp
652: stmfd sp!, {ip} // save svc_ssp
653:
654: #if USE_DBGSPT
655: mov ip, #0
656: stmfd sp!, {ip} // exinf = 0
657: ldr ip, =hook_enter_jmp // hook processing
658: ldr pc, [ip]
659: ret_hook_enter:
660: #endif
661:
662: ldr lr, [fp, #0*4] // lr = function code
663: cmp lr, #0 // < 0: system call
664: bge l_esvc_function // >= 0: extended system call
665:
666: /* T-Kernel system call */
667: ldr ip, =TASKINDP // if called from task-independent portion
668: ldr ip, [ip] // protection-level check is unnecessary
669: cmp ip, #0
670: bhi l_nochklevel
671:
672: ldr ip, =Csym(svc_call_limit)
673: ldr ip, [ip] // limit value of the ring for call protection
674: and r10, r10, #TMF_CPL(3) // r10 is taskmode upon entry
675: cmp r10, ip
676: bhi l_oacv_err
677: l_nochklevel:
678:
679: mov r10, lr, asr #16 // r10 = function number
680: ldr ip, =N_TFN + 0xffff8000
681: cmp r10, ip
682: bgt l_illegal_svc
683:
684: mov lr, lr, lsr #8
685: and lr, lr, #0xff // lr = number of arguments
686: subs lr, lr, #4
687: bicle sp, sp, #7 // align stack module 8 bytes
688: ble l_nocopy
689: sub sp, sp, lr, lsl #2
690: bic sp, sp, #7 // align stack module 8 bytes
691: l_param_copy:
692: subs lr, lr, #1
693: ldr ip, [r4, lr, lsl #2] // copy the arguments (from the fifth and later)
694: str ip, [sp, lr, lsl #2]
695: bgt l_param_copy
696: l_nocopy:
697:
698: ldr ip, =_svctbl - (0xffff8000 << 2)
699: mov lr, pc
700: ldr pc, [ip, r10, lsl #2] // T-Kernel system call
701:
702: l_retsvc:
703: #if USE_DBGSPT
704: ldr ip, =hook_leave_jmp // hook processing
705: ldr pc, [ip]
706: ret_hook_leave:
707: #endif
708:
709: sub sp, fp, #5*4
710: ldmfd sp!, {r2-r3} // svc_ssp, taskmode
711:
712: str r2, [r9, #TCB_tskctxb + CTXB_svc_ssp] // restore svc_ssp
713: mov r2, r9 // r2 = ctxtsk
714:
715: ldr ip, =TASKMODE // restore taskmode
716: str r3, [ip]
717:
718: ldmfd sp!, {r9-r10, fp} // restore working registers
719:
720: ands r3, r3, #TMF_CPL(3) // DCT is not processed if called from
721: beq l_nodct // protection level 0
722:
723: cpsid IMASK // disable interrupt
724:
725: ldr ip, =TASKINDP // DCT is not processed if called from
726: ldr ip, [ip] // task independent portion
727: cmp ip, #0
728: bne l_nodct
729:
730: ldr ip, [r2, #TCB_reqdct] // is there DCT request?
731: cmp ip, #1
732: bne l_nodct
733:
734: ldr ip, =Csym(dispatch_disabled)
735: ldr ip, [ip] // if dispatch is disabled,
736: cmp ip, #0 // DCT is not processed
737: bne l_nodct
738:
739: b dct_startup // goto DCT processing
740:
741: l_nodct:
742: EXC_RETURN
743:
744:
745: l_esvc_function:
746: /* extended SVC */
747: mov ip, #0
748: str ip, [r9, #TCB_tskctxb + CTXB_svc_ssp] // svc_ssp = NULL
749:
750: bic sp, sp, #7 // align stack module 8 bytes
751: mov r1, lr // r1 = function code
752: bl Csym(svc_ientry) // svc_ientry(pk_para, fncd)
753:
754: b l_retsvc
755:
756:
757: l_illegal_svc:
758: ldr r0, =E_RSFN
759: b l_retsvc
760:
761: l_oacv_err:
762: ldr r0, =E_OACV
763: b l_retsvc
764:
765: .data
766: .balign 4
767: dummy_svc_ssp: .long -1
768:
769:
770: #if USE_DBGSPT
771: /*
772: * Invoking hook routine for system call, and extended SVC
773: * void* enter( FN fncd, TD_CALINF *calinf, ... )
774: * void leave( FN fncd, INT ret, void *exinf )
775: *
776: * typedef struct td_calinf {
777: * void *ssp; system stack pointer
778: * void *r11; frame pointer when it is called
779: * } TD_CALINF;
780: */
781: .text
782: .balign 4
783: hook_enter:
784: stmfd sp!, {r0-r3, r8} // save argument and work registers
785: mov r8, sp // r8 = keep the stack position
786:
787: ldr ip, [fp, #-1*4] // frame pointer when it is called
788: stmfd sp!, {fp, ip} // create TD_CALINF
789:
790: bic sp, sp, #7 // align stack module 8 bytes
791:
792: ldr lr, [fp, #0*4] // lr = function code
793: cmp lr, #0 // < 0: system call
794: bge he_param2 // >= 0: extended system call
795:
796: mov lr, lr, lsr #8
797: and lr, lr, #0xff // number of arguments
798: subs lr, lr, #2
799: ble he_param2
800: subs lr, lr, #2
801: ble he_param4
802: sub sp, sp, lr, lsl #2
803: bic sp, sp, #7 // align stack module 8 bytes
804: he_param_copy:
805: subs lr, lr, #1
806: ldr ip, [r4, lr, lsl #2] // Fifth argument (and later)
807: str ip, [sp, lr, lsl #2]
808: bgt he_param_copy
809: he_param4:
810: str r3, [sp, #-1*4]! // forth argument
811: str r2, [sp, #-1*4]! // third argument
812: he_param2:
813: mov r3, r1 // second argument
814: mov r2, r0 // first argument
815: sub r1, r8, #2*4 // calinf
816: ldr r0, [fp, #0*4] // fncd
817: ldr ip, =Csym(hook_enterfn)
818: ldr ip, [ip]
819: blx ip // exinf = enter(fncd, ...)
820: str r0, [fp, #-6*4] // save exinf
821:
822: mov sp, r8 // restore stack position
823: ldmfd sp!, {r0-r3, r8} // restore argument and work registers
824: b ret_hook_enter
825:
826: hook_leave:
827: sub sp, fp, #6*4 // sp = saved position of exinf
828: swp r2, r0, [sp] // save ret and , restore exinf
829: mov r1, r0 // ret
830: ldr r0, [fp, #0*4] // fncd
831:
832: bic sp, sp, #7 // align stack module 8 bytes
833: ldr ip, =Csym(hook_leavefn)
834: ldr ip, [ip]
835: blx ip // call leave(fncd, ret, exinf)
836:
837: ldr r0, [fp, #-6*4] // restore ret
838: b ret_hook_leave
839:
840: /*
841: * Setting and releasing of hook routine for system call and extended SVC
842: */
843: .text
844: .balign 4
845: .globl Csym(hook_svc)
846: .type Csym(hook_svc), %function
847: Csym(hook_svc):
848: ldr r0, =hook_enter_jmp
849: ldr r1, =hook_leave_jmp
850: ldr r2, =hook_enter
851: ldr r3, =hook_leave
852: str r2, [r0]
853: str r3, [r1]
854: bx lr
855:
856: .globl Csym(unhook_svc)
857: .type Csym(unhook_svc), %function
858: Csym(unhook_svc):
859: ldr r0, =hook_enter_jmp
860: ldr r1, =hook_leave_jmp
861: ldr r2, =ret_hook_enter
862: ldr r3, =ret_hook_leave
863: str r2, [r0]
864: str r3, [r1]
865: bx lr
866:
867: .data
868: .balign 4
869: hook_enter_jmp: .long ret_hook_enter
870: hook_leave_jmp: .long ret_hook_leave
871:
872: #endif /* USE_DBGSPT */
873:
874: /* ------------------------------------------------------------------------ */
875:
876: #if USE_DBGSPT
877: /*
878: * Call entry table for debugger support functions
879: */
880: .text
881: .balign 4
882: _tdsvctbl:
883: .int Csym(no_support)
884: #include <sys/svc/tdsvctbl.h>
885:
886: /*
887: * Entry routine for debugger support functions
888: * +---------------+
889: * ssp -> | ip | function code
890: * | lr | return address
891: * | SPSR |
892: * +---------------+
893: */
894: .text
895: .balign 4
896: .globl Csym(call_dbgspt)
897: .type Csym(call_dbgspt), %function
898: Csym(call_dbgspt):
899: /* SVC mode / interrupt-disabled CPSR.I=1 A=? F=? */
900: ldr ip, [sp, #2*4]
901: and ip, ip, #PSR_I|PSR_F // cpsr_c doesn't include PSR_A
902: orr ip, ip, #PSR_SVC
903: msr cpsr_c, ip // interrupt disabled status is restored to the original state in the caller
904:
905: stmfd sp!, {r10, fp} // save work registers
906: add fp, sp, #2*4
907:
908: ldr ip, =TASKINDP // if called from task-independent portion
909: ldr ip, [ip] // no need to check protection levels.
910: cmp ip, #0
911: bhi b_nochklevel
912:
913: ldr ip, =TASKMODE
914: ldr lr, =Csym(svc_call_limit)
915: ldr ip, [ip]
916: ldr lr, [lr] // limit value of the ring for call protection
917: and ip, ip, #TMF_CPL(3) // protection level upon call
918: cmp ip, lr
919: bhi b_oacv_err
920: b_nochklevel:
921:
922: ldr lr, [fp, #0*4] // lr = function code
923: mov r10, lr, asr #16
924: ldr ip, =N_TDFN + 0xffff8000
925: cmp r10, ip
926: bgt b_illegal_svc
927:
928: bic sp, sp, #7 // align stack module 8 bytes
929:
930: ldr ip, =_tdsvctbl - (0xffff8000 << 2)
931: mov lr, pc
932: ldr pc, [ip, r10, lsl #2] // T-Kernel/DS service call
933:
934: sub sp, fp, #2*4 // restore sp
935:
936: b_retsvc:
937: cpsid IMASK // disable interrupt
938: ldmfd sp!, {r10, fp} // restore work registers
939: EXC_RETURN
940:
941:
942: b_illegal_svc:
943: ldr r0, =E_RSFN
944: b b_retsvc
945:
946: b_oacv_err:
947: ldr r0, =E_OACV
948: b b_retsvc
949:
950: #endif /* USE_DBGSPT */
951:
952: /* ------------------------------------------------------------------------ */
953: /*
954: * High-level language support routine for timer handler
955: */
956: .text
957: .balign 4
958: .globl Csym(timer_handler_startup)
959: .type Csym(timer_handler_startup), %function
960: Csym(timer_handler_startup):
961: /* IRQ mode / interrupt-disabled state CPSR.I=1 A=? */
962: cpsid IMASK, #PSR_SVC // enter SVC mode, interrupt is disabled
963:
964: stmfd sp!, {r0-r2, r4-r5, fp, lr} // save registers
965: mov fp, sp
966: bic sp, sp, #7 // align stack module 8 bytes
967:
968: ldr r4, =TASKINDP // enter task-independent portion
969: ldr r5, [r4]
970: add r0, r5, #1
971: str r0, [r4]
972:
973: bl Csym(timer_handler) // call timer_handler()
974: /* return, interrupt disabled, CPSR.I=1 A=1 */
975:
976: str r5, [r4] // leave task-independent portion
977:
978: mov sp, fp
979: ldmfd sp!, {r0-r2, r4-r5, fp, lr} // restore registers
980:
981: TK_RET_INT_FIQ PSR_IRQ // tk_ret_int()
982:
983: /* ------------------------------------------------------------------------ */
984: /*
985: * delayed context trap(DCT)
986: * invoke task exception handler
987: * the stack looks as follows when this is called.
988: * +---------------+
989: * ssp -> | R12 = ip |
990: * | R14_svc = lr | return address(pc)
991: * | SPSR_svc |
992: * +---------------+
993: */
994: .text
995: .balign 4
996: dct_startup:
997: /* SVC mode / interrupt-disabled-state CPSR.I=1 A=1 */
998: stmfd sp!, {r0-r3, fp, lr} // save registers
999: add fp, sp, #6*4
1000: bic sp, sp, #7 // align stack module 8 bytes
1001:
1002: ldr ip, [fp, #2*4] // spsr
1003: and ip, ip, #PSR_M(31) // processor mode of caller
1004: cmp ip, #PSR_USR // was it user mode?
1005: cmpne ip, #PSR_SYS // If not system mode,
1006: bne l_nodct_startup // DCT is not invoked
1007:
1008: mov r0, fp
1009: bl Csym(setup_texhdr) // call setup_texhdr(ssp)
1010: /* return with interrupt-enabled state */
1011: cpsid IMASK // Inhibit interrupt
1012:
1013: l_nodct_startup:
1014: sub sp, fp, #6*4
1015: ldmfd sp!, {r0-r3, fp, lr} // Restore registers
1016: EXC_RETURN
1017:
1018: /*
1019: * return from task exception handler
1020: * +---------------+
1021: * usp -> |PC | return address from the handler
1022: * |CPSR | CPSR to be restored on return from the handler
1023: * +---------------+
1024: *
1025: * +---------------+
1026: * ssp -> | R12 = ip |
1027: * | R14_svc = lr | <- copy PC
1028: * | SPSR_svc | <- copy CPSR
1029: * +---------------+
1030: *
1031: * if an illegal call is made, call default handler.
1032: */
1033: .text
1034: .balign 4
1035: .globl Csym(rettex_entry)
1036: .type Csym(rettex_entry), %function
1037: Csym(rettex_entry):
1038: /* SVC mode / interrupt-disabled state CPSR.I=1 A=? F=? */
1039: ldr ip, [sp, #2*4] // spsr
1040: tst ip, #PSR_A|PSR_I|PSR_F // call during an interrupt-disabled state is illegal
1041: bne l_illegal_rettex
1042: and ip, ip, #PSR_M(31)
1043: cmp ip, #PSR_USR // call neither from user mode nor system mode
1044: cmpne ip, #PSR_SYS // is illegal
1045: bne l_illegal_rettex
1046:
1047: cpsid IMASK, #PSR_SYS // interrupt-disabled state, SYS mode
1048: mov ip, sp // ip = usp
1049:
1050: /* we access user stack, and so we must not be in
1051: task-independent portion, and interrupt-disabled state */
1052: cpsie IMASK, #PSR_SVC // enable interrupt, SVC mode
1053:
1054: ldr lr, [ip, #0*4] // copy usp to ssp
1055: str lr, [sp, #1*4]
1056: ldr ip, [ip, #1*4] // usp->CPSR
1057: ldr lr, [sp, #2*4] // ssp->SPSR
1058: bic ip, ip, #PSR_M(31) // make sure mode is legal
1059: bic ip, ip, #PSR_A|PSR_I|PSR_F
1060: and lr, lr, #PSR_M(31)
1061: orr ip, ip, lr
1062: str ip, [sp, #2*4]
1063:
1064: cpsid IMASK, #PSR_SYS // interrupt-disabled, SYS mode
1065: add sp, sp, #2*4 // pop user stack
1066:
1067: cpsid IMASK, #PSR_SVC // intterupt-disabled, SVC mode
1068: EXC_RETURN
1069:
1070: l_illegal_rettex:
1071: ldr ip, =EITVEC(SWI_RETTEX)
1072: ldr lr, =base(EITVEC(EIT_DEFAULT))
1073: ldr lr, [lr, #offs(EITVEC(EIT_DEFAULT))]
1074: bx lr
1075:
1076: /* ------------------------------------------------------------------------ */