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