#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);
  }
  **/

}