4 files changed, 64 insertions, 31 deletions
diff --git a/src/kernel/kernel.c b/src/kernel/kernel.c
index 50b8359..d550e37 100644
--- a/src/kernel/kernel.c
+++ b/src/kernel/kernel.c
@@ -20,7 +20,10 @@
 #include <testmalloc.h>
 
 
-
+void multicore_main() {
+  printf("\nKernal started\n");
+  asm("cli\nhlt");
+}
 
 void main() {
 #ifndef SCREEN_OUTPUT
@@ -28,7 +31,6 @@ void main() {
 #endif
   get_mem_capabilities(); 
   init_pmap(map_complete_physical()); 
-  unmap_lowmem();
  
   
   find_root_sdp();
@@ -40,6 +42,8 @@ void main() {
   randinit();
 
   smp_boot();
+  fix_stack();
+  unmap_lowmem();
 
   PANIC(KERNEL_PANIC_KERNEL_RETURNED);
 }
diff --git a/src/kernel/page.c b/src/kernel/page.c
index 9db8660..734194c 100644
--- a/src/kernel/page.c
+++ b/src/kernel/page.c
@@ -77,8 +77,7 @@ void get_mem_capabilities() {
   NX_capable = (edx >> 20) & 1;
 }
 
-
-void unmap_lowmem() {
+void fix_stack() {
   struct stack_frame *frame;
 
   asm("addq rsp, %0\n"
@@ -92,7 +91,10 @@ void unmap_lowmem() {
     frame->next = PHYS_TO_VIRT((void *)frame->next);
     frame = frame->next;
   }
+}
 
+
+void unmap_lowmem() {
   //[future] 
   //eventually, you should use the function that unmaps pages when you write it
   page_table *entry = (page_table *)PAGEMAP_LOCATION;
diff --git a/src/kernel/smp.c b/src/kernel/smp.c
index d3c74fe..5796ab9 100644
--- a/src/kernel/smp.c
+++ b/src/kernel/smp.c
@@ -1,9 +1,12 @@
 #include <kernel.h>
+#include <printf.h>
 #include <madt.h>
 #include <timer.h>
 #include <cpuid.h>
 #include <int.h>
 #include <libc.h>
+#include <paging.h>
+#include <heap.h>
 
 #define LAPIC_ICR_LOW   192
 #define LAPIC_ICR_HIGH  196
@@ -11,6 +14,8 @@
 extern lapic_t lapic;
 extern char __load_start_smp_bootloader, __load_stop_smp_bootloader;
 extern uint8_t *smp_bootstrap_corecount;
+extern uint8_t smp_bootstrap_bsp;
+extern uint64_t *smp_bootstrap_stackarray;
 
 struct icr_reg {
   uint8_t vector;
@@ -37,19 +42,24 @@ static inline void write_icr(uint8_t dest, uint32_t message) {
 }
 
 void smp_boot() {
-  uint8_t cores_active = 0; //TODO change in asm
+  uint8_t cores_active = 1; //TODO change in asm
   struct cores_info cores;
-  unsigned int ci;
-
   struct icr_reg icr;
+  get_coreinfo(&cores);
   bzero(&icr, sizeof(icr));
 
+  void **core_stacks = malloc(sizeof(void *) * (cores.corecount - 1));
+  for(unsigned int s = 0; s < (cores.corecount - 1); s++) {
+    core_stacks[s] = palloc(0x1000);
+  }
+
   memcpy(PHYS_TO_VIRT((void *)0x8000), PHYS_TO_VIRT(&__load_start_smp_bootloader), 
       &__load_stop_smp_bootloader - &__load_start_smp_bootloader);
-  *(uint8_t **)PHYS_TO_VIRT(&smp_bootstrap_corecount) = &cores_active;
+  smp_bootstrap_corecount = &cores_active;
+  smp_bootstrap_bsp = cores.bsp;
+  smp_bootstrap_stackarray = (uint64_t)core_stacks;
 
 
-  get_coreinfo(&cores);
 
   icr.deliv_mode = 0b101;
   icr.dest_shorthand = 0b11;
@@ -61,20 +71,13 @@ void smp_boot() {
   icr.vector = 8;
   write_icr(0, *(uint32_t *)&icr);
   usleep(200);
-  write_icr(0, *(uint32_t *)&icr);
-
-  /**
+  if(cores_active != cores.corecount) write_icr(0, *(uint32_t *)&icr);
+  usleep(200);
   if(cores_active != cores.corecount) {
-    write_icr(0, *(uint32_t *)&icr);
-    usleep(200);
+    printf("NOT ALL CORES ONLINE\n");
+    asm("cli\nhlt");
   }
-  **/
-  /**
-  for(ci = 0; ci < cores.corecount; ci++) {
-    if(ci == cores.bsp) continue;
-    //send init sipi
-    write_icr(ci, *(uint32_t *)&icr);
+  else {
+    printf("%i!!!!!!\n", cores_active);
   }
-  **/
-
 }
diff --git a/src/kernel/smp_trampoline.asm b/src/kernel/smp_trampoline.asm
index f723aa6..15b85e2 100644
--- a/src/kernel/smp_trampoline.asm
+++ b/src/kernel/smp_trampoline.asm
@@ -1,10 +1,12 @@
-global smp_bootstrap_corecount;
+global smp_bootstrap_stackarray
+global smp_bootstrap_bsp
+global smp_bootstrap_corecount
+
+extern multicore_main
 
 [bits 16]
 smp_trampoline:
 cli
-hlt
-hlt
 
 xor ax, ax
 mov ds, ax
@@ -50,7 +52,7 @@ SMP_PROTECTED_DATA_SEGMENT equ .gdt_data_p - .gdt_start_p
 
 ;________________________________________________________________________________________
 
-.smp_protected
+.smp_protected:
 
 [bits 32]
 mov ax, SMP_PROTECTED_DATA_SEGMENT
@@ -60,6 +62,9 @@ mov es, ax
 mov fs, ax
 mov gs, ax
 
+lgdt [.gdt_descriptor_l]
+
+
 mov eax, 0x10000 ;TODO clarify _why_ this is a thing
 mov cr3, eax
 
@@ -82,12 +87,14 @@ or eax, 1 << 8 | 1 << 11
 or eax, edx
 wrmsr
 
+
 ;enable paging 
 mov eax, cr0
 or eax, 1 << 31 | 1 << 0; 
 and ax, ~(1 << 2)
 mov cr0, eax
 
+
 jmp SMP_LONG_CODE_SEGMENT:.counter
 
 ;________________________________________________________________________________________
@@ -123,7 +130,7 @@ db 0
 .gdt_end_l: ; later calculates offset in defs below
 
 
-.descriptor_l: 
+.gdt_descriptor_l: 
   dw .gdt_end_l - .gdt_start_l - 1 
 dq .gdt_start_l
 
@@ -131,10 +138,27 @@ SMP_LONG_CODE_SEGMENT equ .gdt_code_l - .gdt_start_l
 SMP_LONG_DATA_SEGMENT equ .gdt_data_l - .gdt_start_l
 
 .counter:
+mov eax, 1
+cpuid
+shr ebx, 24
+cmp bl, BYTE [smp_bootstrap_bsp]
+jl .apic_below_bsp
+sub bl, 1
+
+.apic_below_bsp:
+
+mov rax, QWORD [smp_bootstrap_stackarray]
+debug:
+mov rsp, QWORD [rax + rbx * 8]
+
 mov rax, [smp_bootstrap_corecount]
-inc QWORD [rax]
-cli
-hlt
-;TODO set up stack; enter kernel
+inc BYTE [rax]
+
+mov rax, multicore_main
+jmp rax
 
+align 8 
+smp_bootstrap_stackarray: dq 0
+smp_bootstrap_bsp: db 0
 smp_bootstrap_corecount: db 0
+times 512 - ($ - $$) db 0