arch/arm-native/kernel/kernel_cpu.c

   1 /*
   2     Copyright © 2013-2015, 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
  11 #include <proto/kernel.h>
  12
  13 #include "etask.h"
  14
  15 #include "kernel_intern.h"
  16 #include "kernel_debug.h"
  17 #include "kernel_cpu.h"
  18 #include "kernel_syscall.h"
  19 #include "kernel_scheduler.h"
  20 #include "kernel_intr.h"
  21
  22 #define D(x)
  23 #define DREGS(x)
  24
  25 extern struct Task *sysIdleTask;
  26 uint32_t __arm_affinitymask __attribute__((section(".data"))) = 1;
  27
  28 void cpu_Register()
  29 {
  30     uint32_t tmp;
  31
  32     asm volatile ("mrc p15, 0, %0, c1, c0, 0" : "=r"(tmp));
  33     tmp &= ~1;                                  /* Disable MMU */
  34     tmp |= (1 << 2) | (1 << 12) | (1 << 11);    /* I and D caches, branch prediction */
  35     tmp = (tmp & ~2) | (1 << 22);               /* Unaligned access enable */
  36     asm volatile ("mcr p15, 0, %0, c1, c0, 0" : : "r"(tmp));
  37
  38     cpu_Init(&__arm_arosintern, NULL);
  39
  40     asm volatile (" mrc p15, 0, %0, c0, c0, 5 " : "=r" (tmp));
  41
  42     __arm_affinitymask |= (1 << (tmp & 0x3));
  43
  44     asm volatile("wfi");
  45 }
  46
  47 void cpu_Delay(int usecs)
  48 {
  49     unsigned int delay;
  50     for (delay = 0; delay < usecs; delay++) asm volatile ("mov r0, r0\n");
  51 }
  52
  53 void cpu_Probe(struct ARM_Implementation *krnARMImpl)
  54 {
  55     uint32_t tmp;
  56
  57     asm volatile ("mrc p15, 0, %0, c0, c0, 0" : "=r" (tmp));
  58     if ((tmp & 0xfff0) == 0xc070)
  59     {
  60         krnARMImpl->ARMI_Family = 7;
  61
  62         // Read the Multiprocessor Affinity Register (MPIDR)
  63         asm volatile ("mrc p15, 0, %0, c0, c0, 5" : "=r" (tmp));
  64
  65         if (tmp & (2 << 30))
  66         {
  67             //Multicore system
  68         }
  69     }
  70     else
  71         krnARMImpl->ARMI_Family = 6;
  72
  73     krnARMImpl->ARMI_Delay = &cpu_Delay;
  74 }
  75
  76 void cpu_Init(struct ARM_Implementation *krnARMImpl, struct TagItem *msg)
  77 {
  78     register unsigned int fpuflags;
  79
  80     core_SetupMMU(msg);
  81
  82     /* Enable Vector Floating Point Calculations */
  83     asm volatile("mrc p15,0,%[fpuflags],c1,c0,2\n" : [fpuflags] "=r" (fpuflags));   // Read Access Control Register
  84     fpuflags |= (VFPSingle | VFPDouble);                                            // Enable Single & Double Precision
  85     asm volatile("mcr p15,0,%[fpuflags],c1,c0,2\n" : : [fpuflags] "r" (fpuflags)); // Set Access Control Register
  86     asm volatile(
  87         "       mov %[fpuflags],%[vfpenable]    \n"                                 // Enable VFP
  88         "       fmxr fpexc,%[fpuflags]          \n"
  89          : [fpuflags] "=r" (fpuflags) : [vfpenable] "I" (VFPEnable));
  90 }
  91
  92 void cpu_Switch(regs_t *regs)
  93 {
  94     struct Task *task;
  95
  96     D(bug("[Kernel] cpu_Switch()\n"));
  97
  98     task = SysBase->ThisTask;
  99
 100     /* Copy current task's context into the ETask structure */
 101     /* Restore the task's state */
 102     STORE_TASKSTATE(task, regs)
 103
 104     if (__arm_arosintern.ARMI_GetTime)
 105     {
 106         /* Update the taks CPU time .. */
 107         GetIntETask(task)->iet_CpuTime += __arm_arosintern.ARMI_GetTime() - GetIntETask(task)->iet_private1;
 108     }
 109
 110     core_Switch();
 111 }
 112
 113 void cpu_Dispatch(regs_t *regs)
 114 {
 115     struct Task *task;
 116
 117     D(bug("[Kernel] cpu_Dispatch()\n"));
 118
 119     /* Break Disable() if needed */
 120     if (SysBase->IDNestCnt >= 0) {
 121         SysBase->IDNestCnt = -1;
 122         ((uint32_t *)regs)[13] &= ~0x80;
 123     }
 124
 125     if (!(task = core_Dispatch()))
 126         task = sysIdleTask;
 127
 128     D(bug("[Kernel] cpu_Dispatch: Letting '%s' run for a bit..\n", task->tc_Node.ln_Name));
 129
 130     /* Restore the task's state */
 131     RESTORE_TASKSTATE(task, regs)
 132
 133     DREGS(cpu_DumpRegs(regs));
 134
 135     /* Handle tasks's flags */
 136     if (task->tc_Flags & TF_EXCEPT)
 137         Exception();
 138
 139     if (__arm_arosintern.ARMI_GetTime)
 140     {
 141         /* Store the launch time */
 142         GetIntETask(task)->iet_private1 = __arm_arosintern.ARMI_GetTime();
 143     }
 144
 145     if (task->tc_Flags & TF_LAUNCH)
 146     {
 147         AROS_UFC1(void, task->tc_Launch,
 148                   AROS_UFCA(struct ExecBase *, SysBase, A6));
 149     }
 150 }
 151
 152 void cpu_DumpRegs(regs_t *regs)
 153 {
 154     int i;
 155
 156     bug("[KRN] Register Dump:\n");
 157     for (i = 0; i < 12; i++)
 158     {
 159         bug("[KRN]      r%02d: 0x%08x\n", i, ((uint32_t *)regs)[i]);
 160     }
 161     bug("[KRN] (ip) r12: 0x%08x\n", ((uint32_t *)regs)[12]);
 162     bug("[KRN] (sp) r13: 0x%08x\n", ((uint32_t *)regs)[13]);
 163     bug("[KRN] (lr) r14: 0x%08x\n", ((uint32_t *)regs)[14]);
 164     bug("[KRN] (pc) r15: 0x%08x\n", ((uint32_t *)regs)[15]);
 165     bug("[KRN]     cpsr: 0x%08x\n", ((uint32_t *)regs)[16]);
 166 }