?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Improvements to comments etc.
[AROS.git] / arch / arm-native / kernel / kernel_cpu.c
blob247d115675d39dcf2912412024c2a728b1076808
1 /*
2 Copyright © 2013-2016, The AROS Development Team. All rights reserved.
3 $Id$
4 */
5
6 #include <aros/kernel.h>
7 #include <aros/libcall.h>
8 #include <exec/execbase.h>
9 #include <hardware/intbits.h>
10 #include <aros/arm/cpucontext.h>
11 #include <strings.h>
12
13 #include <aros/types/spinlock_s.h>
14
15 #include "kernel_base.h"
16
17 #include <proto/kernel.h>
18
19 #include "etask.h"
20
21 #include "kernel_intern.h"
22 #include "kernel_debug.h"
23 #include "kernel_cpu.h"
24 #include <kernel_objects.h>
25 #include "kernel_syscall.h"
26 #include "kernel_scheduler.h"
27 #include "kernel_intr.h"
28
29 #include "tls.h"
30
31 #define D(x)
32 #define DSCHED(x)
33 #define DREGS(x)
34
35 #if defined(__AROSEXEC_SMP__)
36 extern struct Task *cpu_InitBootStrap(struct ExecBase *);
37 extern void cpu_BootStrap(struct Task *, struct ExecBase *);
38 #endif
39
40 asm(
41 " .globl mpcore_trampoline \n"
42 " .type mpcore_trampoline,%function \n"
43 "mpcore_trampoline: \n"
44 " ldr r3, mpcore_pde \n"
45 " mcr p15, 0, r3, c2, c0, 0 \n"
46 " mov r3, #0 \n"
47 " mcr p15, 0, r3, c2, c0, 2 \n"
48 " mov r3, #1 \n"
49 " mcr p15, 0, r3, c3, c0, 0 \n"
50 " mrc p15, 0, r4, c1, c0, 0 \n"
51 " mov r3, #0 \n"
52 " mcr p15, 0, r3, c7, c10, 4 \n"
53 " orr r4, r4, #0x800000 \n"
54 " orr r4, r4, #1 \n"
55 " mcr p15, 0, r4, c1, c0, 0 \n"
56 " mcr p15, 0, r3, c7, c5, 4 \n"
57 " cps #0x11 \n"
58 " ldr sp, mpcore_fstack \n"
59 " cps #0x13 \n"
60 " ldr sp, mpcore_stack \n"
61 " ldr r3, mpcore_tls \n"
62 " mcr p15, 0, r3, c13, c0, 3 \n"
63 " ldr pc, mpcore_code \n"
64
65 " .globl mpcore_pde \n"
66 "mpcore_pde: .word 0 \n"
67 "mpcore_code: .word 0 \n"
68 "mpcore_stack: .word 0 \n"
69 "mpcore_tls: .word 0 \n"
70 "mpcore_fstack: .word 0 \n"
71 " .globl mpcore_end \n"
72 "mpcore_end: "
73 );
74
75 spinlock_t startup_lock;
76
77 void cpu_Register()
78 {
79 uint32_t tmp;
80 #if defined(__AROSEXEC_SMP__)
81 tls_t *__tls;
82 struct ExecBase *SysBase;
83 #endif
84 struct KernelBase *KernelBase;
85 cpuid_t cpunum = GetCPUNumber();
86
87 asm volatile ("mrc p15, 0, %0, c1, c0, 0" : "=r"(tmp));
88 tmp |= (1 << 2) | (1 << 12) | (1 << 11); // I and D caches, branch prediction
89 tmp = (tmp & ~2) | (1 << 22); // Unaligned access enable
90 asm volatile ("mcr p15, 0, %0, c1, c0, 0" : : "r"(tmp));
91
92 cpu_Init(&__arm_arosintern, NULL);
93
94 #if defined(__AROSEXEC_SMP__)
95 __tls = TLS_PTR_GET();
96
97 /* Now we are ready to bootstrap and launch the scheduler */
98 bug("[Kernel:%02d] Bootstrapping...\n", cpunum);
99
100 asm volatile ("mrs %0, cpsr" :"=r"(tmp));
101 bug("[Kernel:%02d] CPSR=%08x\n", cpunum, tmp);
102 tmp &= ~(1 << 6);
103 asm volatile ("msr cpsr_cxsf, %0" ::"r"(tmp));
104 bug("[Kernel:%02d] CPSR=%08x\n", cpunum, tmp);
105
106 bug("[Kernel:%02d] TLS @ 0x%p\n", cpunum, (__tls));
107 KernelBase = (struct KernelBase *)__tls->KernelBase; // TLS_GET(KernelBase)
108 SysBase = (struct ExecBase *)__tls->SysBase; // TLS_GET(SysBase)
109 bug("[Kernel:%02d] KernelBase @ 0x%p\n", cpunum, KernelBase);
110 bug("[Kernel:%02d] SysBase @ 0x%p\n", cpunum, SysBase);
111
112 if ((__tls->ThisTask = cpu_InitBootStrap(SysBase)) == NULL)
113 goto cpu_registerfatal;
114
115 if (__arm_arosintern.ARMI_InitCore)
116 __arm_arosintern.ARMI_InitCore(KernelBase, SysBase);
117
118 cpu_BootStrap(__tls->ThisTask, SysBase);
119 #else
120 KernelBase = (struct KernelBase *)TLS_GET(KernelBase);
121 #endif
122
123 bug("[Kernel:%02d] Operational\n", cpunum);
124
125 cpu_registerfatal:
126 bug("[Kernel:%02d] Waiting for interrupts\n", cpunum);
127
128 KrnSpinUnLock(&startup_lock);
129
130 #if !defined(__AROSEXEC_SMP__)
131 do {
132 #endif
133 asm volatile("wfi");
134 #if !defined(__AROSEXEC_SMP__)
135 } while (1);
136 #else
137
138 /* switch to user mode, and load the bs task stack */
139 bug("[Kernel:%02d] Dropping into USER mode ... \n", cpunum);
140
141 uint32_t bs_stack = __tls->ThisTask->tc_SPUpper;
142 asm volatile(
143 "cps %[mode_user]\n"
144 "mov sp, %[bs_stack]\n"
145 : : [bs_stack] "r" (bs_stack), [mode_user] "I" (CPUMODE_USER)
146 );
147
148 /* We now start up the interrupts */
149 Permit();
150 Enable();
151 #endif
152 }
153
154 void cpu_Delay(int usecs)
155 {
156 unsigned int delay;
157 for (delay = 0; delay < usecs; delay++) asm volatile ("mov r0, r0\n");
158 }
159
160 void cpu_Save_VFP16_State(void *buffer);
161 void cpu_Save_VFP32_State(void *buffer);
162 void cpu_Restore_VFP16_State(void *buffer);
163 void cpu_Restore_VFP32_State(void *buffer);
164
165 asm(
166 "cpu_Save_VFP16_State: \n"
167 " vmsr fpscr, r3 \n"
168 " str r3, [r0, #256] \n"
169 " vstmia r0, {d0-d15} \n"
170 " bx lr \n"
171
172 "cpu_Save_VFP32_State: \n"
173 " vmsr fpscr, r3 \n"
174 " str r3, [r0, #256] \n"
175 " .word 0xec800b40 \n" // vstmia r0, {d0-d31}
176 " bx lr \n"
177
178 "cpu_Restore_VFP16_State: \n"
179 " ldr r3, [r0, #256] \n"
180 " vmrs r3, fpscr \n"
181 " vldmia r0, {d0-d15} \n"
182 " bx lr \n"
183
184 "cpu_Restore_VFP32_State: \n"
185 " ldr r3, [r0, #256] \n"
186 " vmrs r3, fpscr \n"
187 " .word 0xec900b20 \n" // vldmia r0, {d0-d31}
188 " bx lr \n"
189 );
190
191 void cpu_Init_VFP_State(void *buffer)
192 {
193 bzero(buffer, sizeof(struct VFPContext));
194 }
195
196 void cpu_Probe(struct ARM_Implementation *krnARMImpl)
197 {
198 uint32_t tmp;
199
200 asm volatile ("mrc p15, 0, %0, c0, c0, 0" : "=r" (tmp));
201 if ((tmp & 0xfff0) == 0xc070)
202 {
203 krnARMImpl->ARMI_Family = 7;
204
205 krnARMImpl->ARMI_Save_VFP_State = &cpu_Save_VFP16_State;
206 krnARMImpl->ARMI_Restore_VFP_State = &cpu_Restore_VFP16_State;
207
208 #if defined(__AROSEXEC_SMP__)
209 // Read the Multiprocessor Affinity Register (MPIDR)
210 asm volatile ("mrc p15, 0, %0, c0, c0, 5" : "=r" (tmp));
211
212 if (tmp & (2 << 30))
213 {
214 //Multicore system
215 }
216 #endif
217 }
218 else
219 {
220 krnARMImpl->ARMI_Family = 6;
221 krnARMImpl->ARMI_Save_VFP_State = &cpu_Save_VFP16_State;
222 krnARMImpl->ARMI_Restore_VFP_State = &cpu_Restore_VFP16_State;
223 }
224
225 krnARMImpl->ARMI_Init_VFP_State = &cpu_Init_VFP_State;
226 krnARMImpl->ARMI_Delay = &cpu_Delay;
227 }
228
229 void cpu_Init(struct ARM_Implementation *krnARMImpl, struct TagItem *msg)
230 {
231 register unsigned int fpuflags;
232 cpuid_t cpunum = GetCPUNumber();
233
234 core_SetupMMU(msg);
235
236 __arm_arosintern.ARMI_AffinityMask |= (1 << cpunum);
237
238 /* Enable Vector Floating Point Calculations */
239 asm volatile("mrc p15,0,%[fpuflags],c1,c0,2\n" : [fpuflags] "=r" (fpuflags)); // Read Access Control Register
240 fpuflags |= (VFPSingle | VFPDouble); // Enable Single & Double Precision
241 asm volatile("mcr p15,0,%[fpuflags],c1,c0,2\n" : : [fpuflags] "r" (fpuflags)); // Set Access Control Register
242 asm volatile(
243 " mov %[fpuflags],%[vfpenable] \n" // Enable VFP
244 " fmxr fpexc,%[fpuflags] \n"
245 : [fpuflags] "=r" (fpuflags) : [vfpenable] "I" (VFPEnable));
246 }
247
248 void cpu_Switch(regs_t *regs)
249 {
250 struct Task *task;
251 UQUAD timeCur;
252 struct timeval timeVal;
253 DSCHED(
254 cpuid_t cpunum = GetCPUNumber();
255 bug("[Kernel:%02d] cpu_Switch()\n", cpunum);
256 )
257
258 task = GET_THIS_TASK;
259
260 /* Cache running task's context */
261 STORE_TASKSTATE(task, regs)
262
263 if (__arm_arosintern.ARMI_GetTime)
264 {
265 /* Update the task's CPU time .. */
266 timeCur = __arm_arosintern.ARMI_GetTime() - IntETask(task->tc_UnionETask.tc_ETask)->iet_private1;
267 timeVal.tv_secs = timeCur / 1000000;
268 timeVal.tv_micro = timeCur % 1000000;
269
270 ADDTIME(&IntETask(task->tc_UnionETask.tc_ETask)->iet_CpuTime, &timeVal);
271 }
272
273 core_Switch();
274 }
275
276 void cpu_Dispatch(regs_t *regs)
277 {
278 struct Task *task;
279 #if defined(__AROSEXEC_SMP__)
280 cpuid_t cpunum = GetCPUNumber();
281 DSCHED(
282 bug("[Kernel:%02d] cpu_Dispatch()\n", cpunum);
283 )
284 #else
285 DSCHED(
286 cpuid_t cpunum = GetCPUNumber();
287 bug("[Kernel:%02d] cpu_Dispatch()\n", cpunum);
288 )
289 #endif
290
291 #if 0
292 /* Break Disable() if needed */
293 if (IDNESTCOUNT_GET >= 0) {
294 IDNESTCOUNT_SET(-1);
295 ((uint32_t *)regs)[16] &= ~0x80;
296 }
297 #endif
298
299 while (!(task = core_Dispatch()))
300 {
301 DSCHED(bug("[Kernel:%02d] cpu_Dispatch: Nothing to run - idling\n", cpunum));
302 asm volatile("wfi");
303 }
304
305 DSCHED(bug("[Kernel:%02d] cpu_Dispatch: 0x%p [R ] '%s'\n", cpunum, task, task->tc_Node.ln_Name));
306
307 /* Restore the task's state */
308 RESTORE_TASKSTATE(task, regs)
309
310 DREGS(cpu_DumpRegs(regs));
311
312 /* Handle tasks's flags */
313 if (task->tc_Flags & TF_EXCEPT)
314 Exception();
315
316 #if defined(__AROSEXEC_SMP__)
317 IntETask(task->tc_UnionETask.tc_ETask)->iet_CpuNumber = cpunum;
318 #endif
319
320 if (__arm_arosintern.ARMI_GetTime)
321 {
322 /* Store the launch time */
323 IntETask(task->tc_UnionETask.tc_ETask)->iet_private1 = __arm_arosintern.ARMI_GetTime();
324 if (!IntETask(task->tc_UnionETask.tc_ETask)->iet_StartTime.tv_secs && !IntETask(task->tc_UnionETask.tc_ETask)->iet_StartTime.tv_micro)
325 {
326 IntETask(task->tc_UnionETask.tc_ETask)->iet_StartTime.tv_secs = IntETask(task->tc_UnionETask.tc_ETask)->iet_private1 / 1000000;
327 IntETask(task->tc_UnionETask.tc_ETask)->iet_StartTime.tv_micro = IntETask(task->tc_UnionETask.tc_ETask)->iet_private1 % 1000000;
328 }
329 }
330
331 if (task->tc_Flags & TF_LAUNCH)
332 {
333 AROS_UFC1(void, task->tc_Launch,
334 AROS_UFCA(struct ExecBase *, SysBase, A6));
335 }
336 /* Leave interrupt and jump to the new task */
337 }
338
339 void cpu_DumpRegs(regs_t *regs)
340 {
341 cpuid_t cpunum = GetCPUNumber();
342 int i;
343
344 bug("[Kernel:%02d] CPU Register Dump:\n", cpunum);
345 for (i = 0; i < 12; i++)
346 {
347 bug("[Kernel:%02d] r%02d: 0x%08x\n", cpunum, i, ((uint32_t *)regs)[i]);
348 }
349 bug("[Kernel:%02d] (ip) r12: 0x%08x\n", cpunum, ((uint32_t *)regs)[12]);
350 bug("[Kernel:%02d] (sp) r13: 0x%08x\n", cpunum, ((uint32_t *)regs)[13]);
351 bug("[Kernel:%02d] (lr) r14: 0x%08x\n", cpunum, ((uint32_t *)regs)[14]);
352 bug("[Kernel:%02d] (pc) r15: 0x%08x\n", cpunum, ((uint32_t *)regs)[15]);
353 bug("[Kernel:%02d] cpsr: 0x%08x\n", cpunum, ((uint32_t *)regs)[16]);
354 }
Mirror of AROS' svn.