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