1 files changed, 35 insertions, 77 deletions

diff --git a/kernel/mp.c b/kernel/mp.c
index e7e811b..f240597 100644
--- a/kernel/mp.c
+++ b/kernel/mp.c
@@ -3,10 +3,8 @@
 #include "x86.h"
 #include "lib/logger/log.h"	// logger facilities
 #include "lib/bool/bool.h"
+#include "smp.h"
 
-#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
@@ -63,33 +61,33 @@ typedef struct proc_struct
 
 }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)
+uint8_t *walk_mp_table(uint8_t *start_addr,smp_processors *smp)
 {
-    /*
-    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);
 
-
+	// that is a processor
 	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");
+	    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");
+
+	    // that is a enabled processor
+	    if(pro->cpu_bits&1)
+	    {
+		if(smp->processors>=SMP_MAX_PROC)
+		    panic(FOOLOS_MODULE_NAME,"we do not support that many processors. recompile with higher SMP_MAX_PROC.");
+
+		smp->local_apic_id[smp->processors]=pro->apic_id;
+		// that is the bootstrap processor
+		if(pro->cpu_bits&2)smp->boot=smp->processors;
+		smp->processors++;
+		
+	    }
 
 	    return start_addr+20;
 	}
@@ -97,61 +95,36 @@ uint8_t *walk_mp_table(uint8_t *start_addr)
 	return start_addr+8;
 }
 
-void show_mp_conf(mp_config *addr)
+void do_mp_conf(mp_config *addr,smp_processors *procdata)
 {
 	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);
+	
+	uint8_t *start_addr=addr;
+	start_addr+=44;
 
-	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=0x7000;
-	*startpos=0xfeeb;
-	*/
-
-	sleep(10);
-
-	log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"STARTING proc 2");
-    
-	*reg=(6<<8)|(1<<14)|0x7; // 110 SIPI
+	procdata->processors=0;
+	procdata->local_apic_address=addr->local_apic;
 
 
-	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);
-
+	    start_addr=walk_mp_table(start_addr,procdata);
 	}
 
+	
+
 
 }
-bool show_mp_fps(mp_fps *addr)
+bool do_mp_fps(mp_fps *addr,smp_processors *procdata)
 {
     
 	if(addr->length!=1)return false;
@@ -167,38 +140,23 @@ bool show_mp_fps(mp_fps *addr)
 	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);
+	do_mp_conf(addr->conf,procdata);
 
 	return true;
 }
 
-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()
+bool mp_find(smp_processors *procdata)
 {
 
     log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"Looking for Mp Floating Ponter Struct...");
     uint8_t *addr=0x8000;
     while(addr<=0xfffff)
     {
-	if(check_mp(addr))
+	if(strcmp("_MP_",addr,4))
 	{
 	//    log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"Found at 0x%04X",addr);
-	    if(show_mp_fps(addr))return true;
+	    if(do_mp_fps(addr,procdata))return true;
 	}
 	addr++;
     }
@@ -206,10 +164,10 @@ bool init_mp()
     addr=0x190000-1025;
     while(addr<=0x190000+1024)
     {
-	if(check_mp(addr))
+	if(strcmp("_MP_",addr,4))
 	{
 	  //  log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"Found at 0x%04X",addr);
-	    if(show_mp_fps(addr))return true;
+	    if(do_mp_fps(addr,procdata))return true;
 	}
 	addr++;
     }