started reactivating multiple processors

1 files changed, 101 insertions, 0 deletions

diff --git a/kernel/smp.c b/kernel/smp.c
new file mode 100644
index 0000000..8d122ee
--- /dev/null
+++ b/kernel/smp.c
@@ -0,0 +1,101 @@
+#define FOOLOS_MODULE_NAME "smp"
+
+#include "lib/logger/log.h"
+#include <stdint.h>
+#include "smp.h"
+#include "mem.h"
+#include "spinlock.h"
+#include "x86.h"
+
+#define FOOLOS_APIC_SPUR_INT 0x00f0
+#define FOOLOS_APIC_INT_COMMAND_LOW 0x0300
+#define FOOLOS_APIC_INT_COMMAND_HIGH 0x0310
+
+// some multiprocessor shit that should move away TODO
+uint32_t c1,c2,c3;
+volatile uint8_t proc;
+uint32_t cpu_counter[SMP_MAX_PROC];
+
+void smp_main()
+{
+    ///	TODO
+    /////// SYMMETRIC MULTIPROCESSING, APS get caought here, move it away ///
+    // catch the APs (Application Processors)
+//    if(mp==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()
+{
+	proc++;
+	uint8_t p=proc;
+	while(1)
+	{
+	    cpu_counter[p]++;
+
+ 	    lock_spin(0);
+	    if(cpu_counter[p]%1000000==0)log(FOOLOS_MODULE_NAME,FOOLOS_LOG_DEBUG,"cpu[%d] %d",p,cpu_counter[p]);
+	    lock_release(0);
+	}
+}
+void smp_log_procdata(smp_processors *procdata)
+{
+    log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"---- smp -----");
+    for(int i=0;i<procdata->processors;i++)
+    {
+	log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"cpu %d, apic_id: 0x%X, bps: %s, apic_addr:0x%08X",i,procdata->local_apic_id[i],i==procdata->boot?"yes":"no",procdata->local_apic_address);
+    }
+
+}
+
+// this will start all our application processors!
+void smp_start_aps(smp_processors *pros,uint8_t *start_sel)
+{
+
+    //lets copy the code to the bootsector !
+	
+	uint8_t *dest=0x7000;
+	for(int i=0;i<0x100;i++)
+	{
+	    dest[i]=start_sel[i];
+
+	}
+    
+	//bsp (boot processor) enables its local apic
+	uint32_t *reg=pros->local_apic_address+FOOLOS_APIC_SPUR_INT;
+	*reg=0xffffffff; // all bits 1 and interrupt 255
+
+        for(int i=0;i<pros->processors;i++)
+	{
+	    if(pros->boot==i)continue;
+
+	    log(FOOLOS_MODULE_NAME,FOOLOS_LOG_INFO,"starting cpu %d",i);
+
+	    uint8_t dest=pros->local_apic_id[i];
+
+	    reg=pros->local_apic_address+FOOLOS_APIC_INT_COMMAND_HIGH;
+	    *reg=dest<<24; // destination apic.
+
+	    reg=pros->local_apic_address+FOOLOS_APIC_INT_COMMAND_LOW;
+	    *reg=(5<<8)|(1<<14); // 101 INIT
+
+	    // do we really neet this?
+	    // todo: use some real sleep (not implemented yet :( )
+	    //sleep(30);
+
+	    // start proc 0x7 = 0x7000; etc..
+	    *reg=(6<<8)|(1<<14)|0x7; // 110 SIPI
+	}
+}
+
+