#include "kernel.h"
// http://www.intel.com/content/dam/doc/specification-update/64-architecture-x2apic-specification.pdf
// http://download.intel.com/design/chipsets/datashts/29056601.pdf
// http://www.scs.stanford.edu/05au-cs240c/lab/ia32/IA32-3.pdf
// https://wiki.osdev.org/Symmetric_Multiprocessing
// https://wiki.osdev.org/APIC_timer

#include <stdint.h>
#include "smp.h"
#include "mem.h"
#include "spinlock.h"
#include "asm_x86.h"

#define APIC_APICID	0x20
#define APIC_APICVER	0x30
#define APIC_TASKPRIOR	0x80
#define APIC_EOI	0x0B0
#define APIC_LDR	0x0D0
#define APIC_DFR	0x0E0
#define APIC_SPURIOUS	0x0F0
#define APIC_ESR	0x280
#define APIC_ICRL	0x300
#define APIC_ICRH	0x310
#define APIC_LVT_TMR	0x320
#define APIC_LVT_PERF	0x340
#define APIC_LVT_LINT0	0x350
#define APIC_LVT_LINT1	0x360
#define APIC_LVT_ERR	0x370
#define APIC_TMRINITCNT	0x380
#define APIC_TMRCURRCNT	0x390
#define APIC_TMRDIV	0x3E0
#define APIC_LAST	0x38F
#define APIC_DISABLE	0x10000
#define APIC_SW_ENABLE	0x100
#define APIC_CPUFOCUS	0x200
#define APIC_NMI	(4<<8)
#define TMR_PERIODIC	0x20000
#define TMR_BASEDIV	(1<<20)

// some multiprocessor shit that should move away TODO
uint32_t c1,c2,c3;
volatile uint8_t proc;
uint32_t cpu_counter[SMP_MAX_PROC];

uint32_t local_apic_addr;

void writeAPIC(uint32_t offset, uint32_t value)
{
    uint32_t *reg;
    reg=local_apic_addr+offset;
    *reg=value;
}

uint32_t readAPIC(uint32_t offset)
{
    uint32_t *reg;
    reg=local_apic_addr+offset;
    uint32_t value=*reg;
    return value;
}

uint32_t apicID()
{
    return readAPIC(APIC_APICID);
}

void apicEOI()
{
	writeAPIC(0xB0,0);
}

void apicIPI(uint8_t dest, uint8_t number)
{
    writeAPIC(APIC_ICRH,dest<<24); // destination apic bits 24-27
    writeAPIC(APIC_ICRL,number | (1<<14)); // send ipi
}

void apicEnable()
{
    writeAPIC(APIC_SPURIOUS,readAPIC(APIC_SPURIOUS)|0x100);

}

void smp_main()
{
    // setup stack
    uint32_t ebp=kballoc(1);
    asm volatile("mov %0, %%ebp"::"r"(ebp));
    asm volatile("mov %ebp, %esp");
    asm volatile("jmp kernel_ap");
}


void kernel_ap()
{
    klog("smp local apic id: 0x%08X",apicID());
    apicEnable();

    int_install();
    gdt_init();
    
    uint32_t countdown=0x0fffffff; // TODO: calc to be constant, depending on bus speed. (use pit for measurement?)
    writeAPIC(APIC_TMRDIV, 0x3);
    writeAPIC(APIC_LVT_TMR, 200 | TMR_PERIODIC);
    writeAPIC(APIC_TMRINITCNT, countdown);

    x86_sti();

    asm_smp_unlock();

    while(1)
    {
	asm("hlt");
	klog("tack: 0x%08X: 0x%08X",readAPIC(APIC_APICID), readAPIC(APIC_TMRCURRCNT));
   }

//    switch_to_user_mode();
    /*

    while(1);

    uint32_t ebp=pmmngr_alloc_block()+4095;

    asm volatile("mov %0, %%ebp"::"r"(ebp));
    asm volatile("mov %ebp, %esp");
    asm volatile("jmp kernel_ap");

    proc=c1=c2=c3=0;
    for(int i=0;i<SMP_MAX_PROC;i++)cpu_counter[i]=0;
    */
}


void kernel_ap_old()
{
	proc++;
	uint8_t p=proc;
	while(1)
	{
	    cpu_counter[p]++;

 	    //lock_spin(0);
	    if(cpu_counter[p]%1000000==0)klog("cpu[%d] %d",p,cpu_counter[p]);
	    //lock_release(0);

	}
}

// this will start all our application processors!
void smp_start_aps(smp_processors *pros)
{
        // TODO: check if local APIC is present via CPUID (P6 (i686) and above)
	local_apic_addr=pros->local_apic_address;

	klog("bsp local apic id: 0x%08X",apicID());

        for(int i=0;i<pros->processors;i++)
	{
	    if(pros->boot==i)continue;

	    uint8_t dest=pros->local_apic_id[i];
	    klog("starting cpu %d (dest: %d) ",i,dest);

	    uint32_t *reg;

	    reg=local_apic_addr+APIC_ICRH;
	    *reg=dest<<24; // destination apic.

	    reg=local_apic_addr+APIC_ICRL;
	    *reg=(5<<8)|(1<<14); // 101 INIT IPI

	    // TODO: wait 10 milliseconds
	    // https://wiki.osdev.org/Symmetric_Multiprocessing

	    *reg=(6<<8)|(1<<14)|0x7; // 110 SIPI
	    // TODO: poll a flag?(timeout 1ms)
	    // TODO retry 110 SIPI with 1s timeout
	    
	}

    apicEnable(); // bsp seems to be enabled anyway.
    
    writeAPIC(APIC_TMRDIV, 0x3);
    writeAPIC(APIC_TMRINITCNT, 0xFFFFFFFF);
    //asm_pit_sleep_1ms();
    //writeAPIC(APIC_LVT_TMR, APIC_LVT_INT_MASKED); //??
    uint32_t ticksInS = 0xFFFFFFFF - readAPIC(APIC_TMRCURRCNT);

    klog("%d MHz bus speed",ticksInS/1000);

    uint32_t countdown=0x0fffffff; // TODO: calc to be constant, depending on bus speed. (use pit for measurement?)
    writeAPIC(APIC_TMRDIV, 0x3);
    writeAPIC(APIC_LVT_TMR, 200 | TMR_PERIODIC);
    writeAPIC(APIC_TMRINITCNT, countdown);
}