#include "kernel.h"
#include "smp.h"

#include <stdint.h>
#include "gdt.h"
#include "mem.h"
#include "interrupts.h"
#include "kmalloc.h"
#include "spinlock.h"
#include "asm_x86.h"
#include "asm_pit.h"
#include "asm_smp.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)

//
uint32_t c1,c2,c3;
volatile uint8_t proc;
uint32_t cpu_counter[SMP_MAX_PROC];

uint32_t local_apic_addr;
uint32_t io_apic_addr;

uint32_t countdown;

void writeIOAPIC(uint32_t offset, uint32_t value)
{
    uint32_t *reg=io_apic_addr;
    reg[0]=(offset &  0xff);
    reg[4]= value;
}

uint32_t readIOAPIC(uint32_t offset)
{
    uint32_t *reg=io_apic_addr;
    reg[0]=(offset & 0xff);
    return reg[4];
}

void irqIOAPIC(uint32_t irq, uint32_t low, uint32_t high)
{
    writeIOAPIC(0x10+irq*2,low);
    writeIOAPIC(0x11+irq*2,high);
}

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);
}

/** select mode : divisor.
 * 0 - 1
 * 1 - 2
 * 2 - 4
 * 3 - 8
 * 4 - 16
 * 5 - 32
 * 6 - 64
 * 7 - 128
 */

uint32_t probeBusSpeed(uint32_t sel)
{
    uint32_t div[]={1,2,4,8,16,32,64,128};
    uint32_t reg[]={0b1011,0,1,2,3,0b1000,0b1001,0b1010};

    uint32_t divisor=div[sel];

    klog("Probing bus speed for 50ms (div=%d)) ...",divisor);
    writeAPIC(APIC_TMRDIV, reg[sel]);
    writeAPIC(APIC_TMRINITCNT, 0xFFFFFFFF);
//    for(int i=0;i<20;i++)
	asm_pit_sleep_50ms();
    //writeAPIC(APIC_LVT_TMR, APIC_LVT_INT_MASKED); //??
    uint32_t ticksInS = 0xFFFFFFFF - readAPIC(APIC_TMRCURRCNT);
    ticksInS*=20; // adjust to one full second.
    klog("%d MHz (%d Hz) bus speed (ticks=%d)",ticksInS/(1000000/divisor),ticksInS*divisor,ticksInS/20);
    return ticksInS*divisor;
}

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();

    interrupts_install();
    gdt_init();
    
    writeAPIC(APIC_TMRDIV, 0x3);
    writeAPIC(APIC_LVT_TMR,INTERRUPT_APIC_TIMER | 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;
    */
}

// this will start all our application processors!
void smp_start_aps(acpi_information *pros)
{
	fixme("how to support IPI addressing more than 16cpus?");
	fixme("check via cpuid if apic exist?");
	local_apic_addr=pros->local_apic_address;
	io_apic_addr=pros->io_apic_address;

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

	apicEnable(); // bsp apic seems to be enabled anyway.

	uint32_t speed=probeBusSpeed(4); // get bus speed (divisor: 16)

	// setup apic timer
	countdown=speed/16/10; // tick 10 times a second
	writeAPIC(APIC_TMRDIV, 0x3); // divisor 16
	writeAPIC(APIC_LVT_TMR, INTERRUPT_APIC_TIMER | TMR_PERIODIC); // on interrupt 200
	writeAPIC(APIC_TMRINITCNT, countdown);
	countdown*=10;

	// setup IO APIC
	// PIT   irq 00 -> 02 flags 0 -> 0x90
	// kb    irq 01 -> 01 flags ? -> 0x91
	// mouse irq 12 -> 12 flags ? -> 0x92
	
    fixme("use acpi info to setup IOAPIC");
	irqIOAPIC(2,0x90,0x0);
	irqIOAPIC(1,0x91,0x0);
	irqIOAPIC(12,0x92,0x0);

        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
	    
	}
}