1: /*
2: *----------------------------------------------------------------------
3: * micro T-Kernel 3.00.05
4: *
5: * Copyright (C) 2006-2020 by Ken Sakamura.
6: * This software is distributed under the T-License 2.2.
7: *----------------------------------------------------------------------
8: *
9: * Released by TRON Forum(http://www.tron.org) at 2021/11.
10: *
11: *----------------------------------------------------------------------
12: */
13: #include <sys/machine.h>
14: #ifdef CPU_CORE_ARMV7A
15: /*
16: * dispatch.S (ARMv7-A)
17: * Dispatcher
18: */
19:
20: #define _in_asm_source_
21: #include "offset.h"
22:
23: /* ------------------------------------------------------------------------ */
24: /*
25: * Dispatcher
26: * knl_dispatch_entry:
27: * Normal dispatch processing.
28: * knl_dispatch_to_schedtsk:
29: * force dispatch processing.
30: *
31: * Contexts to save
32: * Save registers except for sp to a stack. Save 'sp' to TCB.
33: *
34: * Low Address +---------------+
35: * sp -> | R4 |
36: * | R5 |
37: * | R6 |
38: * | R7 |
39: * | R8 |
40: * | R9 |
41: * | R10 |
42: * | R11 |
43: * +---------------+
44: * | R0 |
45: * | R1 |
46: * | R2 |
47: * | R3 |
48: * | R12(ip) |
49: * | R14_svc(lr) |
50: * | R14_xxx(lr) | Return address (pc)
51: * | SPSR_xxx |
52: * High Address +---------------+
53: */
54:
55: /* ----------------------------------------
56: * force dispatch
57: * ---------------------------------------- */
58: .text
59: .balign 4
60: .arm
61: .globl Csym(knl_dispatch_to_schedtsk)
62: .type Csym(knl_dispatch_to_schedtsk), %function
63: Csym(knl_dispatch_to_schedtsk):
64: /* Interrupt is disabled(CPSR.I=1),during SVC mode */
65: ldr sp, =__tmp_stack_start // Set temporal stack
66:
67: ldr ip, =Csym(knl_dispatch_disabled)
68: ldr r0, =1
69: str r0, [ip] // Dispatch disable
70:
71: ldr r4, =Csym(knl_ctxtsk) // R4: &ctxtsk
72: ldr r0, =0
73:
74: ldr r8, [r4] // R8: ctxtsk
75: str r0, [r4] // ctxtsk = NULL
76: b l_dispatch0
77:
78: /* ----------------------------------------
79: * normal dispatch
80: * ---------------------------------------- */
81: .text
82: .balign 4
83: .arm
84: .globl Csym(knl_dispatch_entry)
85: .type Csym(knl_dispatch_entry), %function
86: Csym(knl_dispatch_entry):
87: /* Interrupt is disabled(CPSR.I=1),during SVC mode */
88: ldr r0, =Csym(knl_dispatch_disabled)
89: ldr r1, =1
90: str r1, [r0] // Dispatch disable
91:
92: // Context save
93: stmfd sp!, {r4-r11} // save registers
94:
95: ldr r4, =Csym(knl_ctxtsk) // R4: &ctxtsk
96: ldr r0, =0
97: ldr r8, [r4] // R8: ctxtsk
98: cmp r8, #0 // ctxtsk == NULL ?
99: strne sp, [r8, #TCB_tskctxb + CTXB_ssp] // Save 'sp' to TCB
100: strne r0, [r4] // ctxtsk = NULL
101:
102: l_dispatch0:
103: ldr r5, =Csym(knl_schedtsk) // R5: &schedtsk
104: ldr r6, =Csym(knl_lowpow_discnt) // R6: &lowpow_discnt
105:
106: l_dispatch1:
107: cpsid ia // disable interrupt
108:
109: ldr r8, [r5] // R8: schedtsk
110: cmp r8, #0 // 'schedtsk' exist?
111: bne l_dispatch2 // yes -> jump
112:
113: /* Because there is no task that should be executed, move to the power-saving mode */
114: ldr ip, [r6] // Is 'low_pow' disabled?
115: cmp ip, #0
116: bleq Csym(low_pow) // call low_pow()
117:
118: cpsie ia // enable interrupt
119: nop
120: b l_dispatch1 // try again
121:
122: l_dispatch2: // Switch to 'schedtsk'
123: str r8, [r4] // r4: &ctxtsk, r8: schedtsk
124:
125: // restore context
126: ldr sp, [r8, #TCB_tskctxb + CTXB_ssp] // Restore 'sp' from TCB
127:
128: #if USE_FPU
129: /* switch VFP context */
130: ldr ip, =Csym(knl_fpu_ctx) // r8: ctxtsk
131: ldr ip, [ip]
132: fmrx r0, fpexc
133: cmp ip, r8
134: orreq r0, r0, #0x40000000 // enable VFP(use current context)
135: bicne r0, r0, #0x40000000 // disable VFP(need context switch)
136: fmxr fpexc, r0
137: #endif /* USE_FPU */
138:
139: ldmfd sp!, {r4-r11} // restore registers
140:
141: ldr r0, =Csym(knl_dispatch_disabled)
142: ldr r1, =0
143: str r1, [r0] // Dispatch enable
144:
145: /* return to task */
146: ldmfd sp!, {r0-r3, ip, lr} // restore registers
147: rfefd sp! // restore SPSR_xxx, pc_xxxx(return from exception)
148:
149: #endif /* CPU_CORE_ARMV7A */