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_coremap __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_coremap |= (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     /* Update the taks CPU time .. */
 111     GetIntETask(task)->iet_CpuTime += *((volatile unsigned int *)(SYSTIMER_CLO)) - GetIntETask(task)->iet_private1;
 112
 113     core_Switch();
 114 }
 115
 116 void cpu_Dispatch(regs_t *regs)
 117 {
 118     struct Task *task;
 119
 120     D(bug("[Kernel] cpu_Dispatch()\n"));
 121
 122     /* Break Disable() if needed */
 123     if (SysBase->IDNestCnt >= 0) {
 124         SysBase->IDNestCnt = -1;
 125         ((uint32_t *)regs)[13] &= ~0x80;
 126     }
 127
 128     if (!(task = core_Dispatch()))
 129         task = sysIdleTask;
 130
 131     D(bug("[Kernel] cpu_Dispatch: Letting '%s' run for a bit..\n", task->tc_Node.ln_Name));
 132
 133     /* Restore the task's state */
 134     RESTORE_TASKSTATE(task, regs)
 135
 136     DREGS(cpu_DumpRegs(regs));
 137
 138     /* Handle tasks's flags */
 139     if (task->tc_Flags & TF_EXCEPT)
 140         Exception();
 141
 142     /* Store the launch time */
 143     GetIntETask(task)->iet_private1 = *((volatile unsigned int *)(SYSTIMER_CLO));
 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 }