1 files changed, 80 insertions, 0 deletions
diff --git a/src/kernel/smp.c b/src/kernel/smp.c
new file mode 100644
index 0000000..d3c74fe
--- /dev/null
+++ b/src/kernel/smp.c
@@ -0,0 +1,80 @@
+#include <kernel.h>
+#include <madt.h>
+#include <timer.h>
+#include <cpuid.h>
+#include <int.h>
+#include <libc.h>
+
+#define LAPIC_ICR_LOW   192
+#define LAPIC_ICR_HIGH  196
+
+extern lapic_t lapic;
+extern char __load_start_smp_bootloader, __load_stop_smp_bootloader;
+extern uint8_t *smp_bootstrap_corecount;
+
+struct icr_reg {
+  uint8_t vector;
+  unsigned int deliv_mode:3;
+  unsigned int dest_mode:1;
+  unsigned int deliv_status:1;
+  unsigned int reserved:1;
+  unsigned int level:1; //0 = de-assert
+  unsigned int trig_mode:1;
+  unsigned int reserved_1:2;
+  unsigned int dest_shorthand:2;
+  unsigned int reserved_2:12;
+}__attribute__((packed));
+
+//1: get bsp number             2
+//2: get list of lapics         3
+//3: copy code                  1
+//4: 
+
+
+static inline void write_icr(uint8_t dest, uint32_t message) {
+  lapic[LAPIC_ICR_HIGH] = (uint32_t)dest << 24;
+  lapic[LAPIC_ICR_LOW] = message;
+}
+
+void smp_boot() {
+  uint8_t cores_active = 0; //TODO change in asm
+  struct cores_info cores;
+  unsigned int ci;
+
+  struct icr_reg icr;
+  bzero(&icr, sizeof(icr));
+
+  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;
+
+
+  get_coreinfo(&cores);
+
+  icr.deliv_mode = 0b101;
+  icr.dest_shorthand = 0b11;
+  icr.level = 1;
+  write_icr(0, *(uint32_t *)&icr);
+  usleep(10000);
+
+  icr.deliv_mode = 0b110;
+  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);
+  }
+  **/
+  /**
+  for(ci = 0; ci < cores.corecount; ci++) {
+    if(ci == cores.bsp) continue;
+    //send init sipi
+    write_icr(ci, *(uint32_t *)&icr);
+  }
+  **/
+
+}