#define FOOLOS_MODULE_NAME "mp"

#include "x86.h"
#include "../lib/logger/log.h"	// logger facilities

#define FOOLOS_APIC_SPUR_INT 0x00f0
#define FOOLOS_APIC_INT_COMMAND_LOW 0x0300
#define FOOLOS_APIC_INT_COMMAND_HIGH 0x0310


typedef struct mp_fps_struct
{
    uint32_t	sig;	    //signature "_MP_"
    uint32_t	conf;	    //pointer to config struct
    uint8_t	length;	    //should be 1
    uint8_t	version;    // 1=1.1, 4=1.4

    uint8_t	checksum;

    uint8_t	features1;
    uint8_t	features2;

    uint8_t	res1;	    //reserved
    uint8_t	res2;
    uint8_t	res3;

}mp_fps;

typedef struct mp_config_struct
{
    uint32_t	sig;	    //signature "PCMP"
    uint16_t	length;	    //base table length
    uint8_t	version;    //revision 1=1.1 4=1.4
    uint8_t	checksum;   

    uint32_t	oemid1;	    //OEM id (ascii)
    uint32_t	oemid2;

    uint32_t	prodid1;    //Product id (ascii)
    uint32_t	prodid2;
    uint32_t	prodid3;

    uint32_t	oem_table;  //pointer (optional)
    uint16_t	oem_size;   //size of this table
    uint16_t	entries;    //entry count (following the header)
    uint32_t	local_apic; //local apic address (same for every cpu)
    uint16_t	length_ext; //extended table length (optional)
    uint8_t	check_ext;  //checksum for ext. table

}mp_config;



typedef struct proc_struct
{
    uint8_t type; //0=processor
    uint8_t apic_id;
    uint8_t apic_ver;
    uint8_t cpu_bits;
    uint32_t cpu_sig;
    uint32_t cpu_flags;

}proc_entry;

uint32_t local_apic;

int processors;
uint8_t apic_id[4];



//entries are sorted. (otherwise ignore. bochs!)
uint8_t *walk_mp_table(uint8_t *start_addr)
{
    /*
    static int last=0;
    if(*start_addr<last)return;
    last=*start_addr;
    */
    

       	if(*start_addr==0x0||*start_addr==0x2)
	     log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"entry type: %d",*start_addr);


	if(*start_addr==0x00)
	{
	    proc_entry *pro=start_addr;
	     log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"local apic id: %02X",pro->apic_id);
	     log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"cpu enabled bit: %s",pro->cpu_bits&1?"yes":"no");
	     log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"bootstrap cpu bit: %s",pro->cpu_bits&2?"yes":"no");

	    return start_addr+20;
	}

	return start_addr+8;
}

void show_mp_conf(mp_config *addr)
{
	char buf[]="XXXX";
	uint32_t *buf_addr=buf;
	*buf_addr=addr->sig;

	processors=0;

	log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"mp_config table addr: %08X",addr);
	log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"mp_config signature: %s",buf);
	log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"mp_config version: %02X",addr->version);
	log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"mp_config # of entries: %d",addr->entries);
	log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"mp_config local apic addr: 0x%08X",addr->local_apic);
	log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"mp_config tabel length: %d",addr->length);

	local_apic=addr->local_apic;

	//bsp (boot processor) enables its local apic
	uint32_t *reg=addr->local_apic+FOOLOS_APIC_SPUR_INT;
	*reg=0xffffffff; // all bits 1 and interrupt 255

	reg=addr->local_apic+FOOLOS_APIC_INT_COMMAND_HIGH;
	*reg=1<<24; // destination apic 1!

	reg=addr->local_apic+FOOLOS_APIC_INT_COMMAND_LOW;
	//*reg=(5<<8)&(1<<14)&99;
	//*reg=(6<<8)|(1<<14)|1;
	*reg=(5<<8)|(1<<14); // 101 INIT

	uint16_t *startpos=0xa000;
	*startpos=0xfeeb;

	sleep(10);

	log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"STARTING proc 2");
    
	*reg=(6<<8)|(1<<14)|0xa; // 110 SIPI


	uint8_t *start_addr=addr;
	start_addr+=44;
	for(int i=0;i<addr->entries;i++)
	{
//	    log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"go 0x%08X",start_addr);
	    start_addr=walk_mp_table(start_addr);

	}


}
void show_mp_fps(mp_fps *addr)
{
    
	if(addr->length!=1)return;
	if(addr->version!=1&&addr->version!=4)return;

	char buf[]="XXXX";
	uint32_t *buf_addr=buf;
	*buf_addr=addr->sig;

	log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"signature: %s",buf);
	log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"conf: %08X",addr->conf);
	log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"ver: %02X",addr->version);
	log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"f1: %02X",addr->features1);

	if(addr->features1!=0)panic(FOOLOS_MODULE_NAME,"Intel default config not supported yet!");
	show_mp_conf(addr->conf);
}

bool check_mp(uint8_t *addr)
{
    char test[]="_MP_";

    for(int i=0;i<4;i++)
    {
	if(test[i]!=*(addr+i))return false;

    }

    return true;

}


// todo: check checksum,version etc. and narrow down search
bool init_mp()
{

    log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"Looking for Mp Floating Ponter Struct...");
    uint8_t *addr=0x8000;
    while(addr<=0xfffff)
    {
	if(check_mp(addr))
	{
	//    log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"Found at 0x%04X",addr);
	    show_mp_fps(addr);
	}
	addr++;
    }

    addr=0x190000-1025;
    while(addr<=0x190000+1024)
    {
	if(check_mp(addr))
	{
	  //  log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"Found at 0x%04X",addr);
	    show_mp_fps(addr);
	}
	addr++;
    }

    return true;

}