Added SMP support

Architecture/amd64/cpu/GlobalDescriptorTable.cpp

@@ -1,6 +1,7 @@
 #include "gdt.hpp"
 
 #include <memory.hpp>
+#include <smp.hpp>
 #include <cpu.hpp>
 #include <debug.h>
 
@@ -16,7 +17,7 @@ namespace GlobalDescriptorTable
 
     GlobalDescriptorTableDescriptor gdt = {.Length = sizeof(GlobalDescriptorTableEntries) - 1, .Entries = &GDTEntries};
 
-    TaskStateSegment tss[256] = {
+    TaskStateSegment tss[MAX_CPU] = {
         0,
         {0, 0, 0},
         0,

Architecture/amd64/cpu/SymmetricMultiprocessing.cpp

@@ -1,8 +1,16 @@
 #include <smp.hpp>
 
+#include <interrupts.hpp>
+#include <memory.hpp>
 #include <cpu.hpp>
 
 #include "../../../kernel.h"
+#if defined(__amd64__)
+#include "../Architecture/amd64/acpi.hpp"
+#include "../Architecture/amd64/cpu/apic.hpp"
+#elif defined(__i386__)
+#elif defined(__aarch64__)
+#endif
 
 extern "C" uint64_t _trampoline_start, _trampoline_end;
 
@@ -19,8 +27,129 @@ enum SMPTrampolineAddress
 
 volatile bool CPUEnabled = false;
 
+static __attribute__((aligned(PAGE_SIZE))) CPUData CPUs[MAX_CPU] = {0};
+
+CPUData *GetCPU(uint64_t id) { return &CPUs[id]; }
+
+CPUData *GetCurrentCPU()
+{
+    uint64_t ret = 0;
+#if defined(__amd64__)
+    ret = ((APIC::APIC *)Interrupts::apic)->Read(APIC::APIC::APIC_ID) >> 24;
+#elif defined(__i386__)
+#elif defined(__aarch64__)
+#endif
+
+    if (!CPUs[ret].IsActive)
+    {
+        error("CPU %d is not active!", ret);
+        return &CPUs[0];
+    }
+
+    if (CPUs[ret].Checksum != CPU_DATA_CHECKSUM)
+    {
+        error("CPU %d data is corrupted!", ret);
+        return &CPUs[0];
+    }
+    return &CPUs[ret];
+}
+
 extern "C" void StartCPU()
 {
+    CPU::Interrupts(CPU::Disable);
+    uint64_t CPU_ID;
+#if defined(__amd64__)
+    // Enable CPU features
+    {
+        CPU::x64::CR0 cr0 = CPU::x64::readcr0();
+        CPU::x64::CR4 cr4 = CPU::x64::readcr4();
+        uint32_t rax, rbx, rcx, rdx;
+        CPU::x64::cpuid(0x1, &rax, &rbx, &rcx, &rdx);
+        if (rdx & CPU::x64::CPUID_FEAT_RDX_SSE)
+        {
+            cr0.EM = 0;
+            cr0.MP = 1;
+            cr4.OSFXSR = 1;
+            cr4.OSXMMEXCPT = 1;
+        }
+
+        // Enable cpu cache but... how to use it?
+        cr0.NW = 0;
+        cr0.CD = 0;
+
+        CPU::x64::cpuid(0x1, &rax, &rbx, &rcx, &rdx);
+        if (rdx & CPU::x64::CPUID_FEAT_RDX_UMIP)
+        {
+            fixme("Not going to enable UMIP.");
+            // cr4.UMIP = 1;
+        }
+        if (rdx & CPU::x64::CPUID_FEAT_RDX_SMEP)
+            cr4.SMEP = 1;
+        if (rdx & CPU::x64::CPUID_FEAT_RDX_SMAP)
+            cr4.SMAP = 1;
+        CPU::x64::writecr0(cr0);
+        CPU::x64::writecr4(cr4);
+        CPU::x64::wrmsr(CPU::x64::MSR_CR_PAT, 0x6 | (0x0 << 8) | (0x1 << 16));
+    }
+
+    // Enable APIC
+    {
+        CPU::x64::wrmsr(CPU::x64::MSR_APIC_BASE, (CPU::x64::rdmsr(CPU::x64::MSR_APIC_BASE) | 0x800) & ~(1 << 10));
+        ((APIC::APIC *)Interrupts::apic)->Write(APIC::APIC::APIC_SVR, ((APIC::APIC *)Interrupts::apic)->Read(APIC::APIC::APIC_SVR) | 0x1FF);
+    }
+
+    // Set CPU_ID variable using APIC
+    CPU_ID = ((APIC::APIC *)Interrupts::apic)->Read(APIC::APIC::APIC_ID) >> 24;
+
+    // Initialize GDT and IDT
+    Interrupts::Initialize(CPU_ID);
+#elif defined(__i386__)
+#elif defined(__aarch64__)
+#endif
+    CPU::Interrupts(CPU::Enable);
+    KPrint("CPU %d is online", CPU_ID);
     CPUEnabled = true;
     CPU::Stop();
 }
+
+namespace SMP
+{
+    void Initialize(void *madt)
+    {
+#if defined(__amd64__)
+        for (uint8_t i = 0; i < ((ACPI::MADT *)madt)->CPUCores; i++)
+            if ((((APIC::APIC *)Interrupts::apic)->Read(APIC::APIC::APIC_ID) >> 24) != ((ACPI::MADT *)madt)->lapic[i]->ACPIProcessorId)
+            {
+                ((APIC::APIC *)Interrupts::apic)->Write(APIC::APIC::APIC_ICRHI, (((ACPI::MADT *)madt)->lapic[i]->APICId << 24));
+                ((APIC::APIC *)Interrupts::apic)->Write(APIC::APIC::APIC_ICRLO, 0x500);
+
+                Memory::Virtual().Map(0x0, 0x0, Memory::PTFlag::RW | Memory::PTFlag::US);
+
+                uint64_t TrampolineLength = (uintptr_t)&_trampoline_end - (uintptr_t)&_trampoline_start;
+                for (uint64_t i = 0; i < (TrampolineLength / PAGE_SIZE) + 2; i++)
+                    Memory::Virtual().Map((void *)(TRAMPOLINE_START + (i * PAGE_SIZE)), (void *)(TRAMPOLINE_START + (i * PAGE_SIZE)), Memory::PTFlag::RW | Memory::PTFlag::US);
+
+                memcpy((void *)TRAMPOLINE_START, &_trampoline_start, TrampolineLength);
+
+                POKE(volatile uint64_t, PAGE_TABLE) = CPU::x64::readcr3().raw;
+                POKE(volatile uint64_t, STACK) = (uint64_t)KernelAllocator.RequestPage();
+
+                asm volatile("sgdt [0x580]\n"
+                             "sidt [0x590]\n");
+
+                POKE(volatile uint64_t, START_ADDR) = (uintptr_t)&StartCPU;
+
+                ((APIC::APIC *)Interrupts::apic)->Write(APIC::APIC::APIC_ICRHI, (((ACPI::MADT *)madt)->lapic[i]->APICId << 24));
+                ((APIC::APIC *)Interrupts::apic)->Write(APIC::APIC::APIC_ICRLO, 0x600 | ((uint32_t)TRAMPOLINE_START / PAGE_SIZE));
+
+                while (!CPUEnabled)
+                    ;
+
+                trace("CPU %d loaded.", ((ACPI::MADT *)madt)->lapic[i]->APICId);
+                CPUEnabled = false;
+            }
+#elif defined(__i386__)
+#elif defined(__aarch64__)
+#endif
+    }
+}

Core/Interrupts/IntManager.cpp

@@ -16,12 +16,12 @@
 namespace Interrupts
 {
 #if defined(__amd64__)
-    APIC::APIC *apic = nullptr;
+    /* APIC::APIC */ void *apic = nullptr;
 #elif defined(__i386__)
 #elif defined(__aarch64__)
 #endif
 
-    void Initialize()
+    void Initialize(int Core)
     {
 #if defined(__amd64__)
         GlobalDescriptorTable::Init(0);

Kernel.cpp

@@ -7,6 +7,7 @@
 #include <lock.hpp>
 #include <time.hpp>
 #include <debug.h>
+#include <smp.hpp>
 
 NEWLOCK(KernelLock);
 
@@ -61,14 +62,17 @@ EXTERNC void Entry(BootInfo *Info)
            BootClock.Hour, BootClock.Minute, BootClock.Second,
            BootClock.Day, BootClock.Month, BootClock.Year);
     KPrint("CPU: \e8822AA%s \e8888FF%s (\e058C19%s\e8888FF)", CPU::Vendor(), CPU::Name(), CPU::Hypervisor());
+    GetCPU(0)->ID = 0;
+    GetCPU(0)->IsActive = true;
+    GetCPU(0)->Checksum = CPU_DATA_CHECKSUM;
     KPrint("Initializing GDT and IDT");
-    Interrupts::Initialize();
+    Interrupts::Initialize(0);
     KPrint("Initializing CPU features");
 #if defined(__amd64__)
     CPU::x64::CR0 cr0 = CPU::x64::readcr0();
     CPU::x64::CR4 cr4 = CPU::x64::readcr4();
     uint32_t rax, rbx, rcx, rdx;
-    CPU::x64::cpuid(1, &rax, &rbx, &rcx, &rdx);
+    CPU::x64::cpuid(0x1, &rax, &rbx, &rcx, &rdx);
     if (rdx & CPU::x64::CPUID_FEAT_RDX_SSE)
     {
         debug("Enabling SSE support...");
@@ -84,7 +88,7 @@ EXTERNC void Entry(BootInfo *Info)
     cr0.CD = 0;
 
     debug("Enabling UMIP, SMEP & SMAP support...");
-    CPU::x64::cpuid(1, &rax, &rbx, &rcx, &rdx);
+    CPU::x64::cpuid(0x1, &rax, &rbx, &rcx, &rdx);
     if (rdx & CPU::x64::CPUID_FEAT_RDX_UMIP)
     {
         KPrint("UMIP is supported.");
@@ -125,6 +129,8 @@ EXTERNC void Entry(BootInfo *Info)
     }
     KPrint("Enabling interrupts");
     Interrupts::Enable();
+    KPrint("Initializing SMP");
+    SMP::Initialize(PowerManager->GetMADT());
     KPrint("\e058C19######## \eE85230END \e058C19########");
     while (1)
         CPU::Halt();

include/interrupts.hpp

@@ -5,7 +5,8 @@
 
 namespace Interrupts
 {
-    void Initialize();
+    extern void *apic;
+    void Initialize(int Core);
     void Enable();
 }
 

include/smp.hpp

@@ -3,6 +3,9 @@
 
 #include <types.h>
 
+/** @brief Maximum supported number of CPU cores by the kernel */
+#define MAX_CPU 256
+
 #define CPU_DATA_CHECKSUM 0xC0FFEE
 
 struct CPUData
@@ -32,4 +35,9 @@ struct CPUData
 CPUData *GetCurrentCPU();
 CPUData *GetCPU(uint64_t ID);
 
+namespace SMP
+{
+    void Initialize(void *madt);
+}
+
 #endif // !__FENNIX_KERNEL_SMP_H__

include/types.h

@@ -47,6 +47,12 @@ typedef __builtin_va_list va_list;
 
 #define offsetof(type, member) __builtin_offsetof(type, member)
 
+#define MIN(a, b) (((a) < (b)) ? (a) : (b))
+#define MAX(a, b) (((a) > (b)) ? (a) : (b))
+
+#define VPOKE(type, address) (*((volatile type *)(address)))
+#define POKE(type, address) (*((type *)(address)))
+
 typedef __INT8_TYPE__ int8_t;
 typedef __INT16_TYPE__ int16_t;
 typedef __INT32_TYPE__ int32_t;