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