arch/x86_64-pc/kernel/smp.c

   1 #include <aros/atomic.h>
   2 #include <asm/io.h>
   3 #include <exec/execbase.h>
   4 #include <exec/memory.h>
   5 #include <proto/exec.h>
   6
   7 #include "kernel_base.h"
   8 #include "kernel_debug.h"
   9 #include "kernel_globals.h"
  10 #include "kernel_intern.h"
  11 #include "kernel_syscall.h"
  12 #include "apic.h"
  13 #include "smp.h"
  14
  15 #define D(x) x
  16 #define DWAKE(x)
  17
  18 extern const void *_binary_smpbootstrap_start;
  19 extern const void *_binary_smpbootstrap_size;
  20
  21 static void smp_Entry(IPTR stackBase, volatile UBYTE *apicready, struct KernelBase *KernelBase)
  22 {
  23     /*
  24      * This is the entry point for secondary cores.
  25      * KernelBase is already set up by the primary CPU, so we can use it.
  26      */
  27     IPTR _APICBase;
  28     UWORD _APICID;
  29     UBYTE _APICNO;
  30
  31     /* Enable fxsave/fxrstor */
  32     wrcr(cr4, rdcr(cr4) | _CR4_OSFXSR | _CR4_OSXMMEXCPT);
  33
  34     /* Find out ourselves */
  35     _APICBase = core_APIC_GetBase();
  36     _APICID   = core_APIC_GetID(_APICBase);
  37     _APICNO   = core_APIC_GetNumber(KernelBase->kb_PlatformData->kb_APIC);
  38
  39     D(bug("[SMP] smp_Entry[0x%02X]: launching on AP APIC ID 0x%02X, base @ 0x%p\n", _APICID, _APICID, _APICBase));
  40     D(bug("[SMP] smp_Entry[0x%02X]: KernelBootPrivate 0x%p, stack base 0x%p\n", _APICID, __KernBootPrivate, stackBase));
  41     D(bug("[SMP] smp_Entry[0x%02X]: Stack base 0x%p, ready lock 0x%p\n", _APICID, stackBase, apicready));
  42
  43     /* Set up GDT and LDT for our core */
  44     core_CPUSetup(_APICID, stackBase);
  45
  46     bug("[SMP] APIC #%u of %u Going IDLE (Halting)...\n", _APICNO + 1, KernelBase->kb_PlatformData->kb_APIC->count);
  47
  48     /* Signal the bootstrap core that we are running */
  49     *apicready = 1;
  50
  51     /*
  52      * Unfortunately at the moment we have nothing more to do.
  53      * The rest of AROS is not SMP-capable. :-(
  54      */
  55     while (1) asm volatile("hlt");
  56 }
  57
  58 static int smp_Setup(struct KernelBase *KernelBase)
  59 {
  60     struct PlatformData *pdata = KernelBase->kb_PlatformData;
  61     /* Low memory header is in the tail of memory list - see kernel_startup.c */
  62     struct MemHeader *lowmem = (struct MemHeader *)SysBase->MemList.lh_TailPred;
  63     APTR smpboot;
  64     struct SMPBootstrap *bs;
  65
  66     D(bug("[SMP] Setup\n"));
  67
  68     /*
  69      * Allocate space for SMP bootstrap code in low memory. Its address must be page-aligned.
  70      * Every CPU starts up in real mode (DAMN CRAP!!!)
  71      */
  72     smpboot = Allocate(lowmem, (unsigned long)&_binary_smpbootstrap_size + PAGE_SIZE - 1);
  73     if (!smpboot)
  74     {
  75         D(bug("[SMP] Failed to allocate space for SMP bootstrap\n"));
  76         return 0;
  77     }
  78
  79     /* Install SMP bootstrap code */
  80     bs = (APTR)AROS_ROUNDUP2((IPTR)smpboot, PAGE_SIZE);
  81     CopyMem(&_binary_smpbootstrap_start, bs, (unsigned long)&_binary_smpbootstrap_size);
  82     pdata->kb_APIC_TrampolineBase = bs;
  83
  84     D(bug("[SMP] Copied APIC bootstrap code to 0x%p\n", bs));
  85
  86     /*
  87      * Store constant arguments in bootstrap's data area
  88      * WARNING!!! The bootstrap code assumes PML4 is placed in a 32-bit memory,
  89      * and there seem to be no easy way to fix this.
  90      * If AROS kickstart is ever loaded into high memory, we would need to take
  91      * a special care about it.
  92      */
  93     bs->Arg3 = (IPTR)KernelBase;
  94     bs->PML4 = __KernBootPrivate->PML4;
  95     bs->IP   = smp_Entry;
  96
  97     return 1;
  98 }
  99
 100 /*
 101  * Here we wake up our secondary cores.
 102  */
 103 static int smp_Wake(struct KernelBase *KernelBase)
 104 {
 105     struct PlatformData *pdata = KernelBase->kb_PlatformData;
 106     struct SMPBootstrap *bs = pdata->kb_APIC_TrampolineBase;
 107     struct APICData *apic = pdata->kb_APIC;
 108     APTR _APICStackBase;
 109     IPTR wakeresult;
 110     UBYTE i;
 111     volatile UBYTE apicready;
 112
 113     D(bug("[SMP] Ready spinlock at 0x%p\n", &apicready));
 114
 115     /* Core number 0 is our bootstrap core, so we start from No 1 */
 116     for (i = 1; i < apic->count; i++)
 117     {
 118         UBYTE apic_id = apic->cores[i].lapicID;
 119
 120         D(bug("[SMP] Launching APIC #%u (ID 0x%02X)\n", i + 1, apic_id));
 121
 122         /*
 123          * First we need to allocate a stack for our CPU.
 124          * We allocate the same three stacks as in core_CPUSetup().
 125          */
 126         _APICStackBase = AllocMem(STACK_SIZE * 3, MEMF_CLEAR);
 127         D(bug("[SMP] Allocated STACK for APIC ID 0x%02X @ 0x%p ..\n", apic_id, _APICStackBase));
 128         if (!_APICStackBase)
 129                 return 0;
 130
 131         /* Give the stack to the CPU */
 132         bs->Arg1 = (IPTR)_APICStackBase;
 133         bs->Arg2 = (IPTR)&apicready;
 134         bs->SP   = _APICStackBase + STACK_SIZE;
 135
 136         /* Initialize 'ready' flag to zero before launching the core */
 137         apicready = 0;
 138
 139         /* Start IPI sequence */
 140         wakeresult = core_APIC_Wake(bs, apic_id, apic->lapicBase);
 141         /* wakeresult != 0 means error */
 142         if (!wakeresult)
 143         {
 144             /*
 145              * Before we proceed we need to make sure that the core has picked up
 146              * its stack and we can reload bootstrap argument area with another one.
 147              * We use a very simple spinlock in order to perform this waiting.
 148              * Previously we have set apicready to 0. When the core starts up,
 149              * it writes 1 there.
 150              */
 151             DWAKE(bug("[SMP] Waiting for APIC #%u to initialise .. ", i + 1));
 152             while (!apicready);
 153
 154             D(bug("[SMP] APIC #%u started up\n", i + 1));
 155         }
 156             D(else bug("[SMP] core_APIC_Wake() failed, status 0x%p\n", wakeresult));
 157     }
 158
 159     D(bug("[SMP] Done\n"));
 160
 161     return 1;
 162 }
 163
 164 int smp_Initialize(void)
 165 {
 166     struct KernelBase *KernelBase = getKernelBase();
 167     struct PlatformData *pdata = KernelBase->kb_PlatformData;
 168
 169     if (pdata->kb_APIC && (pdata->kb_APIC->count > 1))
 170     {
 171         if (!smp_Setup(KernelBase))
 172         {
 173             D(bug("[SMP] Failed to prepare the environment!\n"));
 174
 175             pdata->kb_APIC->count = 1;  /* We have only one workinng CPU */
 176             return 0;
 177         }
 178
 179         return smp_Wake(KernelBase);
 180     }
 181
 182     /* This is not an SMP machine, but it's okay */
 183     return 1;
 184 }