Bugfixes, Cleanup and Rewite started on UDI interface

[tpg/acess2.git] / Kernel / arch / x86 / proc.c

diff --git a/Kernel/arch/x86/proc.c b/Kernel/arch/x86/proc.c
index 98ba7e0..a66a540 100644 (file)
--- a/Kernel/arch/x86/proc.c
+++ b/Kernel/arch/x86/proc.c
@@ -52,8 +52,9 @@ void  Proc_Scheduler();
 // === GLOBALS ===
 // --- Multiprocessing ---
 #if USE_MP
+volatile int   giNumInitingCPUs = 0;
 tMPInfo        *gMPFloatPtr = NULL;
-tIOAPIC        *gpMP_LocalAPIC = NULL;
+tAPIC  *gpMP_LocalAPIC = NULL;
 Uint8  gaAPIC_to_CPU[256] = {0};
 tCPU   gaCPUs[MAX_CPUS];
 #else
@@ -176,6 +177,12 @@ void ArchThreads_Init()
                                Log("\t.CPUSignature = 0x%08x", ents->Proc.CPUSignature);
                                Log("\t.FeatureFlags = 0x%08x", ents->Proc.FeatureFlags);
                                
+                               
+                               if( !(ents->Proc.CPUFlags & 1) ) {
+                                       Log("DISABLED");
+                                       break;
+                               }
+                               
                                // Check if there is too many processors
                                if(giNumCPUs >= MAX_CPUS) {
                                        giNumCPUs ++;   // If `giNumCPUs` > MAX_CPUS later, it will be clipped
@@ -189,7 +196,10 @@ void ArchThreads_Init()
                                giNumCPUs ++;
                                
                                // Send IPI
-                               MP_StartAP( giNumCPUs-1 );
+                               if( !(ents->Proc.CPUFlags & 2) )
+                               {
+                                       MP_StartAP( giNumCPUs-1 );
+                               }
                                
                                break;
                        case 1: // Bus
@@ -235,7 +245,11 @@ void ArchThreads_Init()
                
                if( giNumCPUs > MAX_CPUS ) {
                        Warning("Too many CPUs detected (%i), only using %i of them", giNumCPUs, MAX_CPUS);
+                       giNumCPUs = MAX_CPUS;
                }
+       
+               while( giNumInitingCPUs )
+                       MM_FinishVirtualInit();
                
                Panic("Uh oh... MP Table Parsing is unimplemented\n");
        }
@@ -247,6 +261,7 @@ void ArchThreads_Init()
        #else
        giNumCPUs = 1;
        gTSSs = &gTSS0;
+       MM_FinishVirtualInit();
        #endif
        
        // Initialise Double Fault TSS
@@ -316,13 +331,22 @@ void MP_StartAP(int CPU)
        *(Uint16*)(KERNEL_BASE|0x469) = 0xFFFF;
        outb(0x70, 0x0F);       outb(0x71, 0x0A);       // Warm Reset
        MP_SendIPI(gaCPUs[CPU].APICID, 0, 5);
+       giNumInitingCPUs ++;
 }
 
 void MP_SendIPI(Uint8 APICID, int Vector, int DeliveryMode)
 {
-       Uint32  addr = (Uint)gpMP_LocalAPIC + 0x20 + (APICID<<3);
-       
-       *(Uint32*)addr = ((DeliveryMode & 7) << 8) | (Vector & 0xFF);
+       Uint32  addr = (Uint)gpMP_LocalAPIC + 0x300;
+       Uint32  val;
+       
+       // Hi
+       val = (Uint)APICID << 24;
+       Log("*%p = 0x%08x", addr+0x10, val);
+       *(Uint32*)(addr+0x10) = val;
+       // Low (and send)
+       val = ((DeliveryMode & 7) << 8) | (Vector & 0xFF);
+       Log("*%p = 0x%08x", addr, val);
+       *(Uint32*)addr = val;
 }
 #endif
 
@@ -343,8 +367,7 @@ void Proc_Start()
 tThread *Proc_GetCurThread()
 {
        #if USE_MP
-       gpMP_LocalAPIC->Addr = 0;
-       return gaCPUs[ gaAPIC_to_CPU[gpMP_LocalAPIC->Value.Byte] ].Current;
+       return gaCPUs[ gaAPIC_to_CPU[gpMP_LocalAPIC->ID.Val&0xFF] ].Current;
        #else
        return gCurrentThread;
        #endif
@@ -679,7 +702,7 @@ void Proc_Scheduler(int CPU)
        #if USE_MP
        thread = gaCPUs[CPU].Current;
        #else
-       curThread = gCurrentThread;
+       thread = gCurrentThread;
        #endif
        
        // Reduce remaining quantum and continue timeslice if non-zero
@@ -723,7 +746,7 @@ void Proc_Scheduler(int CPU)
        #endif
        
        // Update Kernel Stack pointer
-       gTSSs[CPU].ESP0 = thread->KernelStack;
+       gTSSs[CPU].ESP0 = thread->KernelStack-4;
        
        // Set address space
        #if USE_PAE