arch/arm-native/kernel/kernel_cpu.c

   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 }