Kernel - General fixing after ARM7 changes
[tpg/acess2.git] / Kernel / arch / x86 / proc.c
index 065e6de..8cff256 100644 (file)
@@ -3,6 +3,7 @@
  * proc.c
  */
 #include <acess.h>
+#include <threads.h>
 #include <proc.h>
 #include <desctab.h>
 #include <mm_virt.h>
 #if USE_MP
 # include <mp.h>
 #endif
+#include <hal_proc.h>
 
 // === FLAGS ===
 #define DEBUG_TRACE_SWITCH     0
+#define DEBUG_DISABLE_DOUBLEFAULT      1
+#define DEBUG_VERY_SLOW_SWITCH 0
 
 // === CONSTANTS ===
-#define        SWITCH_MAGIC    0xFFFACE55      // There is no code in this area
+#define        SWITCH_MAGIC    0xFF5317C8      // FF SWITCH - There is no code in this area
 // Base is 1193182
-#define TIMER_DIVISOR  11931   //~100Hz
+#define TIMER_BASE      1193182
+#if DEBUG_VERY_SLOW_PERIOD
+# define TIMER_DIVISOR 1193    //~10Hz switch, with 10 quantum = 1s per thread
+#else
+# define TIMER_DIVISOR 11932   //~100Hz
+#endif
 
 // === TYPES ===
 #if USE_MP
@@ -27,6 +36,7 @@ typedef struct sCPU
        Uint8   State;  // 0: Unavaliable, 1: Idle, 2: Active
        Uint16  Resvd;
        tThread *Current;
+       tThread *IdleThread;
 }      tCPU;
 #endif
 
@@ -36,36 +46,36 @@ extern tIDT gIDT[];
 extern void APWait(void);      // 16-bit AP pause code
 extern void APStartup(void);   // 16-bit AP startup code
 extern Uint    GetEIP(void);   // start.asm
-extern int     GetCPUNum(void);        // start.asm
+extern Uint    GetEIP_Sched(void);     // proc.asm
 extern Uint32  gaInitPageDir[1024];    // start.asm
-extern void    Kernel_Stack_Top;
-extern tSpinlock       glThreadListLock;
+extern char    Kernel_Stack_Top[];
+extern tShortSpinlock  glThreadListLock;
 extern int     giNumCPUs;
 extern int     giNextTID;
-extern int     giTotalTickets;
-extern int     giNumActiveThreads;
 extern tThread gThreadZero;
-extern tThread *gActiveThreads;
-extern tThread *gSleepingThreads;
-extern tThread *gDeleteThreads;
-extern tThread *Threads_GetNextToRun(int CPU);
-extern void    Threads_Dump(void);
-extern tThread *Threads_CloneTCB(Uint *Err, Uint Flags);
 extern void    Isr8(void);     // Double Fault
-extern void    Proc_ReturnToUser(void);
+extern void    Proc_ReturnToUser(tVAddr Handler, Uint Argument, tVAddr KernelStack);
+extern void    scheduler_return;       // Return address in SchedulerBase
+extern void    IRQCommon;      // Common IRQ handler code
+extern void    IRQCommon_handled;      // IRQCommon call return location
+extern void    GetEIP_Sched_ret;       // GetEIP call return location
 
 // === PROTOTYPES ===
-void   ArchThreads_Init(void);
+//void ArchThreads_Init(void);
 #if USE_MP
 void   MP_StartAP(int CPU);
 void   MP_SendIPI(Uint8 APICID, int Vector, int DeliveryMode);
 #endif
-void   Proc_Start(void);
-tThread        *Proc_GetCurThread(void);
+//void Proc_Start(void);
+//tThread      *Proc_GetCurThread(void);
 void   Proc_ChangeStack(void);
- int   Proc_Clone(Uint *Err, Uint Flags);
+// int Proc_Clone(Uint *Err, Uint Flags);
+Uint   Proc_MakeUserStack(void);
+//void Proc_StartUser(Uint Entrypoint, Uint *Bases, int ArgC, char **ArgV, char **EnvP, int DataSize);
 void   Proc_StartProcess(Uint16 SS, Uint Stack, Uint Flags, Uint16 CS, Uint IP);
-void   Proc_CallFaultHandler(tThread *Thread);
+ int   Proc_Demote(Uint *Err, int Dest, tRegs *Regs);
+//void Proc_CallFaultHandler(tThread *Thread);
+//void Proc_DumpThreadCPUState(tThread *Thread);
 void   Proc_Scheduler(int CPU);
 
 // === GLOBALS ===
@@ -73,7 +83,7 @@ void  Proc_Scheduler(int CPU);
 #if USE_MP
 volatile int   giNumInitingCPUs = 0;
 tMPInfo        *gMPFloatPtr = NULL;
-Uint32 giMP_TimerCount;        // Start Count for Local APIC Timer
+volatile Uint32        giMP_TimerCount;        // Start Count for Local APIC Timer
 tAPIC  *gpMP_LocalAPIC = NULL;
 Uint8  gaAPIC_to_CPU[256] = {0};
 tCPU   gaCPUs[MAX_CPUS];
@@ -81,20 +91,18 @@ tTSS        gaTSSs[MAX_CPUS];       // TSS Array
  int   giProc_BootProcessorID = 0;
 #else
 tThread        *gCurrentThread = NULL;
-#endif
-#if USE_PAE
-Uint32 *gPML4s[4] = NULL;
+tThread        *gpIdleThread = NULL;
 #endif
 tTSS   *gTSSs = NULL;  // Pointer to TSS array
 tTSS   gTSS0 = {0};
 // --- Error Recovery ---
-char   gaDoubleFaultStack[1024];
+char   gaDoubleFaultStack[1024] __attribute__ ((section(".padata")));
 tTSS   gDoubleFault_TSS = {
-       .ESP0 = (Uint)&gaDoubleFaultStack[1023],
+       .ESP0 = (Uint)&gaDoubleFaultStack[1024],
        .SS0 = 0x10,
        .CR3 = (Uint)gaInitPageDir - KERNEL_BASE,
        .EIP = (Uint)Isr8,
-       .ESP = (Uint)&gaDoubleFaultStack[1023],
+       .ESP = (Uint)&gaDoubleFaultStack[1024],
        .CS = 0x08,     .SS = 0x10,
        .DS = 0x10,     .ES = 0x10,
        .FS = 0x10,     .GS = 0x10,
@@ -228,6 +236,8 @@ void ArchThreads_Init(void)
                                }
                                
                                break;
+                       
+                       #if DUMP_MP_TABLES
                        case 1: // Bus
                                entSize = 8;
                                Log("%i: Bus", i);
@@ -265,6 +275,7 @@ void ArchThreads_Init(void)
                        default:
                                Log("%i: Unknown (%i)", i, ents->Type);
                                break;
+                       #endif
                        }
                        ents = (void*)( (Uint)ents + entSize );
                }
@@ -283,9 +294,9 @@ void ArchThreads_Init(void)
        #else
        giNumCPUs = 1;
        gTSSs = &gTSS0;
-       MM_FinishVirtualInit();
        #endif
        
+       #if !DEBUG_DISABLE_DOUBLEFAULT
        // Initialise Double Fault TSS
        gGDT[5].BaseLow = (Uint)&gDoubleFault_TSS & 0xFFFF;
        gGDT[5].BaseMid = (Uint)&gDoubleFault_TSS >> 16;
@@ -296,19 +307,39 @@ void ArchThreads_Init(void)
        gIDT[8].CS = 5<<3;
        gIDT[8].Flags = 0x8500;
        gIDT[8].OffsetHi = 0;
+       #endif
        
        // Set timer frequency
        outb(0x43, 0x34);       // Set Channel 0, Low/High, Rate Generator
        outb(0x40, TIMER_DIVISOR&0xFF); // Low Byte of Divisor
        outb(0x40, (TIMER_DIVISOR>>8)&0xFF);    // High Byte
+       
+       Log("Timer Frequency %i.%03i Hz",
+               TIMER_BASE/TIMER_DIVISOR,
+               ((Uint64)TIMER_BASE*1000/TIMER_DIVISOR)%1000
+               );
+       
+       #if USE_MP
        // Get the count setting for APIC timer
        Log("Determining APIC Count");
        __asm__ __volatile__ ("sti");
        while( giMP_TimerCount == 0 )   __asm__ __volatile__ ("hlt");
        __asm__ __volatile__ ("cli");
-       Log("APIC Count %i\n", giMP_TimerCount);
+       Log("APIC Count %i", giMP_TimerCount);
+       {
+               Uint64  freq = giMP_TimerCount;
+               freq /= TIMER_DIVISOR;
+               freq *= TIMER_BASE;
+               if( (freq /= 1000) < 2*1000)
+                       Log("Bus Frequency %i KHz", freq);
+               else if( (freq /= 1000) < 2*1000)
+                       Log("Bus Frequency %i MHz", freq);
+               else if( (freq /= 1000) < 2*1000)
+                       Log("Bus Frequency %i GHz", freq);
+               else
+                       Log("Bus Frequency %i THz", freq);
+       }
        
-       #if USE_MP
        // Initialise Normal TSS(s)
        for(pos=0;pos<giNumCPUs;pos++)
        {
@@ -318,45 +349,32 @@ void ArchThreads_Init(void)
                gTSSs[pos].SS0 = 0x10;
                gTSSs[pos].ESP0 = 0;    // Set properly by scheduler
                gGDT[6+pos].BaseLow = ((Uint)(&gTSSs[pos])) & 0xFFFF;
-               gGDT[6+pos].BaseMid = ((Uint)(&gTSSs[pos])) >> 16;
+               gGDT[6+pos].BaseMid = ((Uint)(&gTSSs[pos]) >> 16) & 0xFFFF;
                gGDT[6+pos].BaseHi = ((Uint)(&gTSSs[pos])) >> 24;
        #if USE_MP
        }
-       
-       // Start APs
-       for( pos = 0; pos < giNumCPUs; pos ++ )
-       {
-               gaCPUs[pos].Current = NULL;
-               if( pos != giProc_BootProcessorID ) {
-                       MP_StartAP( pos );
-               }
-       }
-       
-       Log("Waiting for APs to come up\n");
-       while( giNumInitingCPUs )       __asm__ __volatile__ ("hlt");
-       MM_FinishVirtualInit();
-       //Panic("Uh oh... MP Table Parsing is unimplemented\n");
        #endif
        
        // Load the BSP's TSS
        __asm__ __volatile__ ("ltr %%ax"::"a"(0x30));
+       // Set Current Thread and CPU Number in DR0 and DR1
+       __asm__ __volatile__ ("mov %0, %%db0"::"r"(&gThreadZero));
+       __asm__ __volatile__ ("mov %0, %%db1"::"r"(0));
        
        #if USE_MP
        gaCPUs[0].Current = &gThreadZero;
        #else
        gCurrentThread = &gThreadZero;
        #endif
+       gThreadZero.CurCPU = 0;
        
-       #if USE_PAE
-       gThreadZero.MemState.PDP[0] = 0;
-       gThreadZero.MemState.PDP[1] = 0;
-       gThreadZero.MemState.PDP[2] = 0;
-       #else
        gThreadZero.MemState.CR3 = (Uint)gaInitPageDir - KERNEL_BASE;
-       #endif
        
        // Create Per-Process Data Block
-       MM_Allocate(MM_PPD_CFG);
+       if( !MM_Allocate(MM_PPD_CFG) )
+       {
+               Panic("OOM - No space for initial Per-Process Config");
+       }
        
        // Change Stacks
        Proc_ChangeStack();
@@ -370,7 +388,7 @@ void MP_StartAP(int CPU)
        // Set location of AP startup code and mark for a warm restart
        *(Uint16*)(KERNEL_BASE|0x467) = (Uint)&APWait - (KERNEL_BASE|0xFFFF0);
        *(Uint16*)(KERNEL_BASE|0x469) = 0xFFFF;
-       outb(0x70, 0x0F);       outb(0x71, 0x0A);       // Warm Reset
+       outb(0x70, 0x0F);       outb(0x71, 0x0A);       // Set warm reset flag
        MP_SendIPI(gaCPUs[CPU].APICID, 0, 5);   // Init IPI
        
        // Delay
@@ -416,8 +434,59 @@ void MP_SendIPI(Uint8 APICID, int Vector, int DeliveryMode)
  */
 void Proc_Start(void)
 {
+       #if USE_MP
+        int    i;
+       #endif
+       
+       #if USE_MP
+       // Start APs
+       for( i = 0; i < giNumCPUs; i ++ )
+       {
+                int    tid;
+               if(i)   gaCPUs[i].Current = NULL;
+               
+               // Create Idle Task
+               if( (tid = Proc_Clone(0, 0)) == 0)
+               {
+                       for(;;) HALT(); // Just yeilds
+               }
+               gaCPUs[i].IdleThread = Threads_GetThread(tid);
+               gaCPUs[i].IdleThread->ThreadName = (char*)"Idle Thread";
+               Threads_SetPriority( gaCPUs[i].IdleThread, -1 );        // Never called randomly
+               gaCPUs[i].IdleThread->Quantum = 1;      // 1 slice quantum
+               
+               
+               // Start the AP
+               if( i != giProc_BootProcessorID ) {
+                       MP_StartAP( i );
+               }
+       }
+       
+       // BSP still should run the current task
+       gaCPUs[0].Current = &gThreadZero;
+       
+       // Start interrupts and wait for APs to come up
+       Log("Waiting for APs to come up\n");
+       __asm__ __volatile__ ("sti");
+       while( giNumInitingCPUs )       __asm__ __volatile__ ("hlt");
+       #else
+       // Create Idle Task
+       if(Proc_Clone(0) == 0)
+       {
+               gpIdleThread = Proc_GetCurThread();
+               gpIdleThread->ThreadName = strdup("Idle Thread");
+               Threads_SetPriority( gpIdleThread, -1 );        // Never called randomly
+               gpIdleThread->Quantum = 1;      // 1 slice quantum
+               for(;;) HALT(); // Just yeilds
+       }
+       
+       // Set current task
+       gCurrentThread = &gThreadZero;
+       
        // Start Interrupts (and hence scheduler)
        __asm__ __volatile__("sti");
+       #endif
+       MM_FinishVirtualInit();
 }
 
 /**
@@ -427,7 +496,6 @@ void Proc_Start(void)
 tThread *Proc_GetCurThread(void)
 {
        #if USE_MP
-       //return gaCPUs[ gaAPIC_to_CPU[gpMP_LocalAPIC->ID.Val&0xFF] ].Current;
        return gaCPUs[ GetCPUNum() ].Current;
        #else
        return gCurrentThread;
@@ -489,7 +557,7 @@ void Proc_ChangeStack(void)
  * \fn int Proc_Clone(Uint *Err, Uint Flags)
  * \brief Clone the current process
  */
-int Proc_Clone(Uint *Err, Uint Flags)
+int Proc_Clone(Uint Flags)
 {
        tThread *newThread;
        tThread *cur = Proc_GetCurThread();
@@ -498,12 +566,17 @@ int Proc_Clone(Uint *Err, Uint Flags)
        __asm__ __volatile__ ("mov %%esp, %0": "=r"(esp));
        __asm__ __volatile__ ("mov %%ebp, %0": "=r"(ebp));
        
-       newThread = Threads_CloneTCB(Err, Flags);
+       newThread = Threads_CloneTCB(NULL, Flags);
        if(!newThread)  return -1;
        
        // Initialise Memory Space (New Addr space or kernel stack)
        if(Flags & CLONE_VM) {
                newThread->MemState.CR3 = MM_Clone();
+               // Check for errors
+               if(newThread->MemState.CR3 == 0) {
+                       Threads_Kill(newThread, -2);
+                       return -1;
+               }
                newThread->KernelStack = cur->KernelStack;
        } else {
                Uint    tmpEbp, oldEsp = esp;
@@ -515,7 +588,7 @@ int Proc_Clone(Uint *Err, Uint Flags)
                newThread->KernelStack = MM_NewKStack();
                // Check for errors
                if(newThread->KernelStack == 0) {
-                       free(newThread);
+                       Threads_Kill(newThread, -2);
                        return -1;
                }
 
@@ -542,7 +615,15 @@ int Proc_Clone(Uint *Err, Uint Flags)
        newThread->SavedState.EBP = ebp;
        eip = GetEIP();
        if(eip == SWITCH_MAGIC) {
-               outb(0x20, 0x20);       // ACK Timer and return as child
+               //__asm__ __volatile__ ("mov %0, %%db0" : : "r" (newThread) );
+               #if USE_MP
+               // ACK the interrupt
+               if( GetCPUNum() )
+                       gpMP_LocalAPIC->EOI.Val = 0;
+               else
+               #endif
+                       outb(0x20, 0x20);       // ACK Timer and return as child
+               __asm__ __volatile__ ("sti");   // Restart interrupts
                return 0;
        }
        
@@ -567,16 +648,13 @@ int Proc_SpawnWorker(void)
        cur = Proc_GetCurThread();
        
        // Create new thread
-       new = malloc( sizeof(tThread) );
+       new = Threads_CloneThreadZero();
        if(!new) {
                Warning("Proc_SpawnWorker - Out of heap space!\n");
                return -1;
        }
-       memcpy(new, &gThreadZero, sizeof(tThread));
-       // Set Thread ID
-       new->TID = giNextTID++;
        // Create a new worker stack (in PID0's address space)
-       // The stack is relocated by this code
+       // - The stack is relocated by this function
        new->KernelStack = MM_NewWorkerStack();
 
        // Get ESP and EBP based in the new stack
@@ -590,14 +668,21 @@ int Proc_SpawnWorker(void)
        new->SavedState.EBP = ebp;
        eip = GetEIP();
        if(eip == SWITCH_MAGIC) {
-               outb(0x20, 0x20);       // ACK Timer and return as child
+               //__asm__ __volatile__ ("mov %0, %%db0" : : "r"(new));
+               #if USE_MP
+               // ACK the interrupt
+               if(GetCPUNum())
+                       gpMP_LocalAPIC->EOI.Val = 0;
+               else
+               #endif
+                       outb(0x20, 0x20);       // ACK Timer and return as child
+               __asm__ __volatile__ ("sti");   // Restart interrupts
                return 0;
        }
        
        // Set EIP as parent
        new->SavedState.EIP = eip;
        // Mark as active
-       new->Status = THREAD_STAT_ACTIVE;
        Threads_AddActive( new );
        
        return new->TID;
@@ -621,7 +706,13 @@ Uint Proc_MakeUserStack(void)
        
        // Allocate Stack - Allocate incrementally to clean up MM_Dump output
        for( i = 0; i < USER_STACK_SZ/0x1000; i++ )
-               MM_Allocate( base + (i<<12) );
+       {
+               if( !MM_Allocate( base + (i<<12) ) )
+               {
+                       Warning("OOM: Proc_MakeUserStack");
+                       return 0;
+               }
+       }
        
        return base + USER_STACK_SZ;
 }
@@ -752,10 +843,94 @@ void Proc_CallFaultHandler(tThread *Thread)
 {
        // Rewinds the stack and calls the user function
        // Never returns
-       __asm__ __volatile__ ("mov %0, %%ebp;\n\tcall Proc_ReturnToUser" :: "r"(Thread->FaultHandler));
+       Proc_ReturnToUser( Thread->FaultHandler, Thread->CurFaultNum, Thread->KernelStack );
        for(;;);
 }
 
+void Proc_DumpThreadCPUState(tThread *Thread)
+{
+       if( Thread->CurCPU > -1 )
+       {
+                int    maxBacktraceDistance = 6;
+               tRegs   *regs = NULL;
+               Uint32  *stack;
+               
+               if( Thread->CurCPU != GetCPUNum() ) {
+                       Log("  Currently running");
+                       return ;
+               }
+               
+               // Backtrace to find the IRQ entrypoint
+               // - This will usually only be called by an IRQ, so this should
+               //   work
+               __asm__ __volatile__ ("mov %%ebp, %0" : "=r" (stack));
+               while( maxBacktraceDistance -- )
+               {
+                       // [ebp] = oldEbp
+                       // [ebp+4] = retaddr
+                       
+                       if( stack[1] == (tVAddr)&IRQCommon_handled ) {
+                               regs = (void*)stack[2];
+                               break;
+                       }
+                       
+                       stack = (void*)stack[0];
+               }
+               
+               if( !regs ) {
+                       Log("  Unable to find IRQ Entry");
+                       return ;
+               }
+               
+               Log("  at %04x:%08x", regs->cs, regs->eip);
+               return ;
+       }
+       
+       #if 1
+       tVAddr  diffFromScheduler = Thread->SavedState.EIP - (tVAddr)Proc_Scheduler;
+       tVAddr  diffFromClone = Thread->SavedState.EIP - (tVAddr)Proc_Clone;
+       tVAddr  diffFromSpawn = Thread->SavedState.EIP - (tVAddr)Proc_SpawnWorker;
+       
+       if( diffFromClone > 0 && diffFromClone < 512 )  // When I last checked, GetEIP was at .+0x183
+       {
+               Log("  Creating full thread");
+               return ;
+       }
+       
+       if( diffFromSpawn > 0 && diffFromSpawn < 512 )  // When I last checked, GetEIP was at .+0x99
+       {
+               Log("  Creating worker thread");
+               return ;
+       }
+       
+       if( diffFromScheduler > 0 && diffFromScheduler < 256 )  // When I last checked, GetEIP was at .+0x60
+       #else
+       Uint32  data[3];
+       MM_ReadFromAddrSpace(Thread->MemState.CR3, Thread->SavedState.EBP, data, 12);
+       if( data[1] == (Uint32)&IRQCommon + 25 )
+       {
+               tRegs   *regs = (void *) data[2];
+               Log("  oldebp = 0x%08x, ret = 0x%08x, regs = 0x%x",
+                       data[0], data[1], data[2]
+                       );
+               // [EBP] = old EBP
+               // [EBP+0x04] = Return Addr
+               // [EBP+0x08] = Arg 1 (CPU Number)
+               // [EBP+0x0C] = Arg 2 (Thread)
+               // [EBP+0x10] = GS (start of tRegs)
+               Log("  IRQ%i from %02x:%08x", regs->int_num regs->cs, regs->eip);
+       }
+       if( stack[1] == (Uint32)&scheduler_return )
+       #endif
+       {
+               // Scheduled out
+               Log("  At %04x:%08x", Thread->SavedState.UserCS, Thread->SavedState.UserEIP);
+               return ;
+       }
+       
+       Log("  Just created (unknow %p)", Thread->SavedState.EIP);
+}
+
 /**
  * \fn void Proc_Scheduler(int CPU)
  * \brief Swap current thread and clears dead threads
@@ -768,24 +943,6 @@ void Proc_Scheduler(int CPU)
        // If the spinlock is set, let it complete
        if(IS_LOCKED(&glThreadListLock))        return;
        
-       // Clear Delete Queue
-       while(gDeleteThreads)
-       {
-               thread = gDeleteThreads->Next;
-               if(gDeleteThreads->IsLocked) {  // Only free if structure is unused
-                       gDeleteThreads->Status = THREAD_STAT_NULL;
-                       free( gDeleteThreads );
-               }
-               gDeleteThreads = thread;
-       }
-       
-       // Check if there is any tasks running
-       if(giNumActiveThreads == 0) {
-               Log("No Active threads, sleeping");
-               __asm__ __volatile__ ("hlt");
-               return;
-       }
-       
        // Get current thread
        #if USE_MP
        thread = gaCPUs[CPU].Current;
@@ -793,37 +950,66 @@ void Proc_Scheduler(int CPU)
        thread = gCurrentThread;
        #endif
        
-       // Reduce remaining quantum and continue timeslice if non-zero
-       if(thread->Remaining--) return;
-       // Reset quantum for next call
-       thread->Remaining = thread->Quantum;
-       
-       // Get machine state
-       __asm__ __volatile__ ("mov %%esp, %0":"=r"(esp));
-       __asm__ __volatile__ ("mov %%ebp, %0":"=r"(ebp));
-       eip = GetEIP();
-       if(eip == SWITCH_MAGIC) return; // Check if a switch happened
+       // NOTE:
+       // 2011-04-05
+       // Bug may be caused by DR0 not being maintained somewhere, hence 
+       // login is getting loaded with the idle state.
+       if( thread )
+       {
+               tRegs   *regs;
+               // Reduce remaining quantum and continue timeslice if non-zero
+               if( thread->Remaining-- )
+                       return;
+               // Reset quantum for next call
+               thread->Remaining = thread->Quantum;
+               
+               // Get machine state
+               __asm__ __volatile__ ( "mov %%esp, %0" : "=r" (esp) );
+               __asm__ __volatile__ ( "mov %%ebp, %0" : "=r" (ebp) );
+               eip = GetEIP();
+               if(eip == SWITCH_MAGIC) return; // Check if a switch happened
+               
+               // Save machine state
+               thread->SavedState.ESP = esp;
+               thread->SavedState.EBP = ebp;
+               thread->SavedState.EIP = eip;
+               
+               // TODO: Make this more stable somehow
+               regs = (tRegs*)(ebp+(2+2)*4);   // EBP,Ret + CPU,CurThread
+               thread->SavedState.UserCS = regs->cs;
+               thread->SavedState.UserEIP = regs->eip;
+               
+               if(thread->bInstrTrace) {
+                       regs->eflags |= 0x100;  // Set TF
+                       Log("%p De-scheduled", thread);
+               }
+               else
+                       regs->eflags &= ~0x100; // Clear TF
+       }
        
-       // Save machine state
-       thread->SavedState.ESP = esp;
-       thread->SavedState.EBP = ebp;
-       thread->SavedState.EIP = eip;
+       // Get next thread to run
+       thread = Threads_GetNextToRun(CPU, thread);
        
-       // Get next thread
-       thread = Threads_GetNextToRun(CPU);
        
-       // Error Check
+       // No avaliable tasks, just go into low power mode (idle thread)
        if(thread == NULL) {
-               Warning("Hmm... Threads_GetNextToRun returned NULL, I don't think this should happen.\n");
-               return;
+               #if USE_MP
+               thread = gaCPUs[CPU].IdleThread;
+               Log("CPU %i Running Idle Thread", CPU);
+               #else
+               thread = gpIdleThread;
+               #endif
        }
        
        #if DEBUG_TRACE_SWITCH
-       Log("Switching to task %i, CR3 = 0x%x, EIP = %p",
-               thread->TID,
-               thread->MemState.CR3,
-               thread->SavedState.EIP
-               );
+       if(thread && thread != Proc_GetCurThread() ) {
+               Log("Switching to task %i(%s), CR3 = 0x%x, EIP = %p",
+                       thread->TID,
+                       thread->ThreadName,
+                       thread->MemState.CR3,
+                       thread->SavedState.EIP
+                       );
+       }
        #endif
        
        // Set current thread
@@ -833,18 +1019,13 @@ void Proc_Scheduler(int CPU)
        gCurrentThread = thread;
        #endif
        
-       //Log("CPU = %i", CPU);
+       #if USE_MP      // MP Debug
+//     Log("CPU = %i, Thread %p", CPU, thread);
+       #endif
        
        // Update Kernel Stack pointer
        gTSSs[CPU].ESP0 = thread->KernelStack-4;
        
-       // Set address space
-       #if USE_PAE
-       # error "Todo: Implement PAE Address space switching"
-       #else
-               __asm__ __volatile__ ("mov %0, %%cr3"::"a"(thread->MemState.CR3));
-       #endif
-       
        #if 0
        if(thread->SavedState.ESP > 0xC0000000
        && thread->SavedState.ESP < thread->KernelStack-0x2000) {
@@ -852,13 +1033,23 @@ void Proc_Scheduler(int CPU)
        }
        #endif
        
+       if( thread->bInstrTrace ) {
+               Log("%p Scheduled", thread);
+       }
+       
+       // Set thread pointer
+       __asm__ __volatile__("mov %0, %%db0\n\t" : : "r"(thread) );
        // Switch threads
        __asm__ __volatile__ (
+               "mov %4, %%cr3\n\t"     // Set address space
                "mov %1, %%esp\n\t"     // Restore ESP
                "mov %2, %%ebp\n\t"     // and EBP
+               "or %5, 72(%%ebp)\n\t"  // or trace flag to eflags (2+2+4+8+2)*4
                "jmp *%3" : :   // And return to where we saved state (Proc_Clone or Proc_Scheduler)
                "a"(SWITCH_MAGIC), "b"(thread->SavedState.ESP),
-               "d"(thread->SavedState.EBP), "c"(thread->SavedState.EIP)
+               "d"(thread->SavedState.EBP), "c"(thread->SavedState.EIP),
+               "r"(thread->MemState.CR3),
+               "r"(thread->bInstrTrace&&thread->SavedState.EIP==(Uint)&GetEIP_Sched_ret?0x100:0)
                );
        for(;;);        // Shouldn't reach here
 }

UCC git Repository :: git.ucc.asn.au