KernelLand/Kernel/arch/x86/proc.c

   1 /*
   2  * AcessOS Microkernel Version
   3  * proc.c
   4  */
   5 #include <acess.h>
   6 #include <threads.h>
   7 #include <proc.h>
   8 #include <desctab.h>
   9 #include <mm_virt.h>
  10 #include <errno.h>
  11 #include <hal_proc.h>
  12 #include <arch_int.h>
  13 #include <proc_int.h>
  14 #if USE_MP
  15 # include <apic.h>
  16 #endif
  17
  18 // === FLAGS ===
  19 #define DEBUG_TRACE_SWITCH      0
  20 #define DEBUG_DISABLE_DOUBLEFAULT       1
  21 #define DEBUG_VERY_SLOW_PERIOD  0
  22 #define DEBUG_NOPREEMPT 1
  23
  24 // === CONSTANTS ===
  25 // Base is 1193182
  26 #define TIMER_BASE      1193182
  27 #if DEBUG_VERY_SLOW_PERIOD
  28 # define TIMER_DIVISOR  1193    //~10Hz switch, with 10 quantum = 1s per thread
  29 #else
  30 # define TIMER_DIVISOR  11932   //~100Hz
  31 #endif
  32
  33 // === TYPES ===
  34
  35 // === IMPORTS ===
  36 extern tGDT     gGDT[];
  37 extern tIDT     gIDT[];
  38 extern void     APWait(void);   // 16-bit AP pause code
  39 extern void     APStartup(void);        // 16-bit AP startup code
  40 extern void     NewTaskHeader(tThread *Thread, void *Fcn, int nArgs, ...);      // Actually takes cdecl args
  41 extern Uint     Proc_CloneInt(Uint *ESP, Uint32 *CR3, int bNoUserClone);
  42 extern Uint32   gaInitPageDir[1024];    // start.asm
  43 extern char     Kernel_Stack_Top[];
  44 extern int      giNumCPUs;
  45 extern tThread  gThreadZero;
  46 extern tProcess gProcessZero;
  47 extern void     Isr8(void);     // Double Fault
  48 extern void     Proc_ReturnToUser(tVAddr Handler, Uint Argument, tVAddr KernelStack);
  49 extern char     scheduler_return[];     // Return address in SchedulerBase
  50 extern char     IRQCommon[];    // Common IRQ handler code
  51 extern char     IRQCommon_handled[];    // IRQCommon call return location
  52 extern char     GetEIP_Sched_ret[];     // GetEIP call return location
  53
  54 // === PROTOTYPES ===
  55 //void  ArchThreads_Init(void);
  56 #if USE_MP
  57 void    MP_StartAP(int CPU);
  58 void    MP_SendIPIVector(int CPU, Uint8 Vector);
  59 void    MP_SendIPI(Uint8 APICID, int Vector, int DeliveryMode);
  60 #endif
  61 void    Proc_IdleThread(void *Ptr);
  62 //void  Proc_Start(void);
  63 //tThread       *Proc_GetCurThread(void);
  64 void    Proc_ChangeStack(void);
  65 // int  Proc_NewKThread(void (*Fcn)(void*), void *Data);
  66 // int  Proc_Clone(Uint *Err, Uint Flags);
  67 Uint    Proc_MakeUserStack(void);
  68 //void  Proc_StartUser(Uint Entrypoint, Uint *Bases, int ArgC, char **ArgV, char **EnvP, int DataSize);
  69 void    Proc_StartProcess(Uint16 SS, Uint Stack, Uint Flags, Uint16 CS, Uint IP) NORETURN;
  70 //void  Proc_CallFaultHandler(tThread *Thread);
  71 //void  Proc_DumpThreadCPUState(tThread *Thread);
  72 void    Proc_Scheduler(int CPU);
  73
  74 // === GLOBALS ===
  75 // --- Multiprocessing ---
  76 #if USE_MP
  77 volatile int    giNumInitingCPUs = 0;
  78 volatile Uint32 giMP_TimerCount;        // Start Count for Local APIC Timer
  79 tAPIC   *gpMP_LocalAPIC = NULL;
  80 Uint8   gaAPIC_to_CPU[256] = {0};
  81  int    giProc_BootProcessorID = 0;
  82 tTSS    gaTSSs[MAX_CPUS];       // TSS Array
  83 #endif
  84 tCPU    gaCPUs[MAX_CPUS] = {
  85         {.Current = &gThreadZero}
  86         };
  87 tTSS    *gTSSs = NULL;  // Pointer to TSS array
  88 tTSS    gTSS0 = {0};
  89 // --- Error Recovery ---
  90 char    gaDoubleFaultStack[1024] __attribute__ ((section(".padata")));
  91 tTSS    gDoubleFault_TSS = {
  92         .ESP0 = (Uint)&gaDoubleFaultStack[1024],
  93         .SS0 = 0x10,
  94         .CR3 = (Uint)gaInitPageDir - KERNEL_BASE,
  95         .EIP = (Uint)Isr8,
  96         .ESP = (Uint)&gaDoubleFaultStack[1024],
  97         .CS = 0x08,     .SS = 0x10,
  98         .DS = 0x10,     .ES = 0x10,
  99         .FS = 0x10,     .GS = 0x10,
 100 };
 101
 102 // === CODE ===
 103 /**
 104  * \fn void ArchThreads_Init(void)
 105  * \brief Starts the process scheduler
 106  */
 107 void ArchThreads_Init(void)
 108 {
 109         // Mark BSP as active
 110         gaCPUs[0].State = 2;
 111
 112         #if USE_MP
 113         // -- Initialise Multiprocessing
 114         const void *mpfloatptr = MPTable_LocateFloatPtr();
 115         if( mpfloatptr )
 116         {
 117                 giNumCPUs = MPTable_FillCPUs(mpfloatptr, gaCPUs, MAX_CPUS, &giProc_BootProcessorID);
 118                 for( int i = 0; i < giNumCPUs; i ++ )
 119                 {
 120                         // TODO: Determine if there's an overlap
 121                         gaAPIC_to_CPU[gaCPUs[i].APICID] = i;
 122                 }
 123                 gTSSs = gaTSSs;
 124         }
 125         else
 126         {
 127                 Log("No MP Table was found, assuming uniprocessor");
 128                 giNumCPUs = 1;
 129                 gTSSs = &gTSS0;
 130         }
 131         #else
 132         giNumCPUs = 1;
 133         gTSSs = &gTSS0;
 134         #endif
 135
 136         #if !DEBUG_DISABLE_DOUBLEFAULT
 137         // Initialise Double Fault TSS
 138         gGDT[5].BaseLow = (Uint)&gDoubleFault_TSS & 0xFFFF;
 139         gGDT[5].BaseMid = (Uint)&gDoubleFault_TSS >> 16;
 140         gGDT[5].BaseHi = (Uint)&gDoubleFault_TSS >> 24;
 141
 142         // Set double fault IDT to use the new TSS
 143         gIDT[8].OffsetLo = 0;
 144         gIDT[8].CS = 5<<3;
 145         gIDT[8].Flags = 0x8500;
 146         gIDT[8].OffsetHi = 0;
 147         #endif
 148
 149         // Set timer frequency
 150         outb(0x43, 0x34);       // Set Channel 0, Low/High, Rate Generator
 151         outb(0x40, TIMER_DIVISOR&0xFF); // Low Byte of Divisor
 152         outb(0x40, (TIMER_DIVISOR>>8)&0xFF);    // High Byte
 153
 154         Log_Debug("Proc", "PIT Frequency %i.%03i Hz",
 155                 TIMER_BASE/TIMER_DIVISOR,
 156                 ((Uint64)TIMER_BASE*1000/TIMER_DIVISOR)%1000
 157                 );
 158
 159         #if USE_MP
 160         // Get the count setting for APIC timer
 161         Log("Determining APIC Count");
 162         __asm__ __volatile__ ("sti");
 163         while( giMP_TimerCount == 0 )   __asm__ __volatile__ ("hlt");
 164         __asm__ __volatile__ ("cli");
 165         Log("APIC Count %i", giMP_TimerCount);
 166         {
 167                 Uint64  freq = giMP_TimerCount;
 168                 freq *= TIMER_BASE;
 169                 freq /= TIMER_DIVISOR;
 170                 if( (freq /= 1000) < 2*1000)
 171                         Log("Bus Frequency %i KHz", freq);
 172                 else if( (freq /= 1000) < 2*1000)
 173                         Log("Bus Frequency %i MHz", freq);
 174                 else if( (freq /= 1000) < 2*1000)
 175                         Log("Bus Frequency %i GHz", freq);
 176                 else
 177                         Log("Bus Frequency %i THz", freq);
 178         }
 179
 180         // Initialise Normal TSS(s)
 181         for(int pos=0;pos<giNumCPUs;pos++)
 182         {
 183         #else
 184         const int pos = 0;
 185         #endif
 186                 gTSSs[pos].SS0 = 0x10;
 187                 gTSSs[pos].ESP0 = 0;    // Set properly by scheduler
 188                 gGDT[6+pos].BaseLow = ((Uint)(&gTSSs[pos])) & 0xFFFF;
 189                 gGDT[6+pos].BaseMid = ((Uint)(&gTSSs[pos]) >> 16) & 0xFFFF;
 190                 gGDT[6+pos].BaseHi = ((Uint)(&gTSSs[pos])) >> 24;
 191         #if USE_MP
 192         }
 193         #endif
 194
 195         // Load the BSP's TSS
 196         __asm__ __volatile__ ("ltr %%ax"::"a"(0x30));
 197         // Set Current Thread and CPU Number in DR0 and DR1
 198         __asm__ __volatile__ ("mov %0, %%db0"::"r"(&gThreadZero));
 199         __asm__ __volatile__ ("mov %0, %%db1"::"r"(0));
 200
 201         gaCPUs[0].Current = &gThreadZero;
 202         gThreadZero.CurCPU = 0;
 203
 204         gProcessZero.MemState.CR3 = (Uint)gaInitPageDir - KERNEL_BASE;
 205
 206         // Create Per-Process Data Block
 207         if( !MM_Allocate(MM_PPD_CFG) )
 208         {
 209                 Panic("OOM - No space for initial Per-Process Config");
 210         }
 211
 212         // Initialise SSE support
 213         Proc_InitialiseSSE();
 214
 215         // Change Stacks
 216         Proc_ChangeStack();
 217 }
 218
 219 #if USE_MP
 220 /**
 221  * \brief Start an AP
 222  */
 223 void MP_StartAP(int CPU)
 224 {
 225         Log_Log("Proc", "Starting AP %i (APIC %i)", CPU, gaCPUs[CPU].APICID);
 226
 227         // Set location of AP startup code and mark for a warm restart
 228         *(Uint16*)(KERNEL_BASE|0x467) = (Uint)&APWait - (KERNEL_BASE|0xFFFF0);
 229         *(Uint16*)(KERNEL_BASE|0x469) = 0xFFFF;
 230         outb(0x70, 0x0F);       outb(0x71, 0x0A);       // Set warm reset flag
 231         MP_SendIPI(gaCPUs[CPU].APICID, 0, 5);   // Init IPI
 232
 233         // Take a quick nap (20ms)
 234         Time_Delay(20);
 235
 236         // TODO: Use a better address, preferably registered with the MM
 237         // - MM_AllocDMA mabye?
 238         // Create a far jump
 239         *(Uint8*)(KERNEL_BASE|0x11000) = 0xEA;  // Far JMP
 240         *(Uint16*)(KERNEL_BASE|0x11001) = (Uint16)(tVAddr)&APStartup + 0x10;    // IP
 241         *(Uint16*)(KERNEL_BASE|0x11003) = 0xFFFF;       // CS
 242
 243         giNumInitingCPUs ++;
 244
 245         // Send a Startup-IPI to make the CPU execute at 0x11000 (which we
 246         // just filled)
 247         MP_SendIPI(gaCPUs[CPU].APICID, 0x11, 6);        // StartupIPI
 248
 249         tTime   timeout = now() + 2;
 250         while( giNumInitingCPUs && now() > timeout )
 251                 HALT();
 252
 253         if( giNumInitingCPUs == 0 )
 254                 return ;
 255
 256         // First S-IPI failed, send again
 257         MP_SendIPI(gaCPUs[CPU].APICID, 0x11, 6);
 258         timeout = now() + 2;
 259         while( giNumInitingCPUs && now() > timeout )
 260                 HALT();
 261         if( giNumInitingCPUs == 0 )
 262                 return ;
 263
 264         Log_Notice("Proc", "CPU %i (APIC %x) didn't come up", CPU, gaCPUs[CPU].APICID);
 265
 266         // Oh dammit.
 267         giNumInitingCPUs = 0;
 268 }
 269
 270 void MP_SendIPIVector(int CPU, Uint8 Vector)
 271 {
 272         MP_SendIPI(gaCPUs[CPU].APICID, Vector, 0);
 273 }
 274
 275 /**
 276  * \brief Send an Inter-Processor Interrupt
 277  * \param APICID        Processor's Local APIC ID
 278  * \param Vector        Argument of some kind
 279  * \param DeliveryMode  Type of signal
 280  * \note 3A 10.5 "APIC/Handling Local Interrupts"
 281  */
 282 void MP_SendIPI(Uint8 APICID, int Vector, int DeliveryMode)
 283 {
 284         Uint32  val;
 285
 286         // Hi
 287         val = (Uint)APICID << 24;
 288         gpMP_LocalAPIC->ICR[1].Val = val;
 289         // Low (and send)
 290         val = ((DeliveryMode & 7) << 8) | (Vector & 0xFF);
 291         gpMP_LocalAPIC->ICR[0].Val = val;
 292 }
 293 #endif
 294
 295 void Proc_IdleThread(void *Ptr)
 296 {
 297         tCPU    *cpu = &gaCPUs[GetCPUNum()];
 298         cpu->Current->ThreadName = strdup("Idle Thread");
 299         Threads_SetPriority( cpu->Current, -1 );        // Never called randomly
 300         cpu->Current->Quantum = 1;      // 1 slice quantum
 301         for(;;) {
 302                 __asm__ __volatile__ ("sti");   // Make sure interrupts are enabled
 303                 __asm__ __volatile__ ("hlt");
 304                 Proc_Reschedule();
 305         }
 306 }
 307
 308 /**
 309  * \fn void Proc_Start(void)
 310  * \brief Start process scheduler
 311  */
 312 void Proc_Start(void)
 313 {
 314         #if USE_MP
 315         // BSP still should run the current task
 316         gaCPUs[giProc_BootProcessorID].Current = &gThreadZero;
 317
 318         __asm__ __volatile__ ("sti");
 319
 320         // Start APs
 321         for( int i = 0; i < giNumCPUs; i ++ )
 322         {
 323                 if(i != giProc_BootProcessorID)
 324                         gaCPUs[i].Current = NULL;
 325
 326                 // Create Idle Task
 327                 Proc_NewKThread(Proc_IdleThread, &gaCPUs[i]);
 328
 329                 // Start the AP
 330                 if( i != giProc_BootProcessorID ) {
 331                         MP_StartAP( i );
 332                 }
 333         }
 334         #else
 335         // Create Idle Task
 336         Proc_NewKThread(Proc_IdleThread, &gaCPUs[0]);
 337
 338         // Set current task
 339         gaCPUs[0].Current = &gThreadZero;
 340
 341         // Start Interrupts (and hence scheduler)
 342         __asm__ __volatile__("sti");
 343         #endif
 344         MM_FinishVirtualInit();
 345 }
 346
 347 /**
 348  * \fn tThread *Proc_GetCurThread(void)
 349  * \brief Gets the current thread
 350  */
 351 tThread *Proc_GetCurThread(void)
 352 {
 353         #if USE_MP
 354         return gaCPUs[ GetCPUNum() ].Current;
 355         #else
 356         return gaCPUs[ 0 ].Current;
 357         #endif
 358 }
 359
 360 /**
 361  * \fn void Proc_ChangeStack(void)
 362  * \brief Swaps the current stack for a new one (in the proper stack reigon)
 363  */
 364 void Proc_ChangeStack(void)
 365 {
 366         Uint    esp, ebp;
 367         Uint    tmpEbp, oldEsp;
 368         Uint    curBase, newBase;
 369
 370         __asm__ __volatile__ ("mov %%esp, %0":"=r"(esp));
 371         __asm__ __volatile__ ("mov %%ebp, %0":"=r"(ebp));
 372
 373         oldEsp = esp;
 374
 375         // Create new KStack
 376         newBase = MM_NewKStack();
 377         // Check for errors
 378         if(newBase == 0) {
 379                 Panic("What the?? Unable to allocate space for initial kernel stack");
 380                 return;
 381         }
 382
 383         curBase = (Uint)&Kernel_Stack_Top;
 384
 385         LOG("curBase = 0x%x, newBase = 0x%x", curBase, newBase);
 386
 387         // Get ESP as a used size
 388         esp = curBase - esp;
 389         LOG("memcpy( %p, %p, 0x%x )", (void*)(newBase - esp), (void*)(curBase - esp), esp );
 390         // Copy used stack
 391         memcpy( (void*)(newBase - esp), (void*)(curBase - esp), esp );
 392         // Get ESP as an offset in the new stack
 393         esp = newBase - esp;
 394         // Adjust EBP
 395         ebp = newBase - (curBase - ebp);
 396
 397         // Repair EBPs & Stack Addresses
 398         // Catches arguments also, but may trash stack-address-like values
 399         for(tmpEbp = esp; tmpEbp < newBase; tmpEbp += 4)
 400         {
 401                 if(oldEsp < *(Uint*)tmpEbp && *(Uint*)tmpEbp < curBase)
 402                         *(Uint*)tmpEbp += newBase - curBase;
 403         }
 404
 405         Proc_GetCurThread()->KernelStack = newBase;
 406
 407         __asm__ __volatile__ ("mov %0, %%esp"::"r"(esp));
 408         __asm__ __volatile__ ("mov %0, %%ebp"::"r"(ebp));
 409 }
 410
 411 void Proc_ClearProcess(tProcess *Process)
 412 {
 413         MM_ClearSpace(Process->MemState.CR3);
 414 }
 415
 416 void Proc_ClearThread(tThread *Thread)
 417 {
 418         if(Thread->SavedState.SSE) {
 419                 free(Thread->SavedState.SSE);
 420                 Thread->SavedState.SSE = NULL;
 421         }
 422 }
 423
 424 tTID Proc_NewKThread(void (*Fcn)(void*), void *Data)
 425 {
 426         Uint    esp;
 427         tThread *newThread;
 428
 429         newThread = Threads_CloneTCB(0);
 430         if(!newThread)  return -1;
 431
 432         // Create new KStack
 433         newThread->KernelStack = MM_NewKStack();
 434         // Check for errors
 435         if(newThread->KernelStack == 0) {
 436                 free(newThread);
 437                 return -1;
 438         }
 439
 440         esp = newThread->KernelStack;
 441         *(Uint*)(esp-=4) = (Uint)Data;  // Data (shadowed)
 442         *(Uint*)(esp-=4) = 1;   // Number of params
 443         *(Uint*)(esp-=4) = (Uint)Fcn;   // Function to call
 444         *(Uint*)(esp-=4) = (Uint)newThread;     // Thread ID
 445
 446         newThread->SavedState.ESP = esp;
 447         newThread->SavedState.EIP = (Uint)&NewTaskHeader;
 448         newThread->SavedState.SSE = NULL;
 449 //      Log("New (KThread) %p, esp = %p", newThread->SavedState.EIP, newThread->SavedState.ESP);
 450
 451 //      MAGIC_BREAK();
 452         Threads_AddActive(newThread);
 453
 454         return newThread->TID;
 455 }
 456
 457 /**
 458  * \fn int Proc_Clone(Uint *Err, Uint Flags)
 459  * \brief Clone the current process
 460  */
 461 tPID Proc_Clone(Uint Flags)
 462 {
 463         tThread *newThread;
 464         tThread *cur = Proc_GetCurThread();
 465         Uint    eip;
 466
 467         // Sanity, please
 468         if( !(Flags & CLONE_VM) ) {
 469                 Log_Error("Proc", "Proc_Clone: Don't leave CLONE_VM unset, use Proc_NewKThread instead");
 470                 return -1;
 471         }
 472
 473         // New thread
 474         newThread = Threads_CloneTCB(Flags);
 475         if(!newThread)  return -1;
 476
 477         newThread->KernelStack = cur->KernelStack;
 478
 479         // Clone state
 480         eip = Proc_CloneInt(&newThread->SavedState.ESP, &newThread->Process->MemState.CR3, Flags & CLONE_NOUSER);
 481         if( eip == 0 ) {
 482                 return 0;
 483         }
 484         newThread->SavedState.EIP = eip;
 485         newThread->SavedState.SSE = NULL;
 486         newThread->SavedState.bSSEModified = 0;
 487
 488         // Check for errors
 489         if( newThread->Process->MemState.CR3 == 0 ) {
 490                 Log_Error("Proc", "Proc_Clone: MM_Clone failed");
 491                 Threads_Delete(newThread);
 492                 return -1;
 493         }
 494
 495         // Add the new thread to the run queue
 496         Threads_AddActive(newThread);
 497         return newThread->TID;
 498 }
 499
 500 /**
 501  * \fn int Proc_SpawnWorker(void)
 502  * \brief Spawns a new worker thread
 503  */
 504 tThread *Proc_SpawnWorker(void (*Fcn)(void*), void *Data)
 505 {
 506         tThread *new;
 507         Uint    stack_contents[4];
 508
 509         // Create new thread
 510         new = Threads_CloneThreadZero();
 511         if(!new) {
 512                 Warning("Proc_SpawnWorker - Out of heap space!\n");
 513                 return NULL;
 514         }
 515
 516         // Create the stack contents
 517         stack_contents[3] = (Uint)Data;
 518         stack_contents[2] = 1;
 519         stack_contents[1] = (Uint)Fcn;
 520         stack_contents[0] = (Uint)new;
 521
 522         // Create a new worker stack (in PID0's address space)
 523         new->KernelStack = MM_NewWorkerStack(stack_contents, sizeof(stack_contents));
 524
 525         // Save core machine state
 526         new->SavedState.ESP = new->KernelStack - sizeof(stack_contents);
 527         new->SavedState.EIP = (Uint)NewTaskHeader;
 528         new->SavedState.SSE = NULL;
 529         new->SavedState.bSSEModified = 0;
 530
 531         // Mark as active
 532         new->Status = THREAD_STAT_PREINIT;
 533         Threads_AddActive( new );
 534
 535         return new;
 536 }
 537
 538 /**
 539  * \fn Uint Proc_MakeUserStack(void)
 540  * \brief Creates a new user stack
 541  */
 542 Uint Proc_MakeUserStack(void)
 543 {
 544          int    i;
 545         Uint    base = USER_STACK_TOP - USER_STACK_SZ;
 546
 547         // Check Prospective Space
 548         for( i = USER_STACK_SZ >> 12; i--; )
 549                 if( MM_GetPhysAddr( (void*)(base + (i<<12)) ) != 0 )
 550                         break;
 551
 552         if(i != -1)     return 0;
 553
 554         // Allocate Stack - Allocate incrementally to clean up MM_Dump output
 555         for( i = 0; i < USER_STACK_SZ/0x1000; i++ )
 556         {
 557                 if( !MM_Allocate( base + (i<<12) ) )
 558                 {
 559                         Warning("OOM: Proc_MakeUserStack");
 560                         return 0;
 561                 }
 562         }
 563
 564         return base + USER_STACK_SZ;
 565 }
 566
 567 void Proc_StartUser(Uint Entrypoint, Uint Base, int ArgC, const char **ArgV, int DataSize)
 568 {
 569         Uint    *stack;
 570          int    i;
 571         const char      **envp = NULL;
 572         Uint16  ss, cs;
 573
 574         // Copy data to the user stack and free original buffer
 575         stack = (void*)Proc_MakeUserStack();
 576         stack -= (DataSize+sizeof(*stack)-1)/sizeof(*stack);
 577         memcpy( stack, ArgV, DataSize );
 578         free(ArgV);
 579
 580         // Adjust Arguments and environment
 581         if( DataSize )
 582         {
 583                 Uint delta = (Uint)stack - (Uint)ArgV;
 584                 ArgV = (const char**)stack;
 585                 for( i = 0; ArgV[i]; i++ )      ArgV[i] += delta;
 586                 envp = &ArgV[i+1];
 587                 for( i = 0; envp[i]; i++ )      envp[i] += delta;
 588         }
 589
 590         // User Mode Segments
 591         ss = 0x23;      cs = 0x1B;
 592
 593         // Arguments
 594         *--stack = (Uint)envp;
 595         *--stack = (Uint)ArgV;
 596         *--stack = (Uint)ArgC;
 597         *--stack = Base;
 598
 599         Proc_StartProcess(ss, (Uint)stack, 0x202, cs, Entrypoint);
 600 }
 601
 602 void Proc_StartProcess(Uint16 SS, Uint Stack, Uint Flags, Uint16 CS, Uint IP)
 603 {
 604         Uint    *stack = (void*)Stack;
 605         *--stack = SS;          //Stack Segment
 606         *--stack = Stack;       //Stack Pointer
 607         *--stack = Flags;       //EFLAGS (Resvd (0x2) and IF (0x20))
 608         *--stack = CS;          //Code Segment
 609         *--stack = IP;  //EIP
 610         //PUSHAD
 611         *--stack = 0xAAAAAAAA;  // eax
 612         *--stack = 0xCCCCCCCC;  // ecx
 613         *--stack = 0xDDDDDDDD;  // edx
 614         *--stack = 0xBBBBBBBB;  // ebx
 615         *--stack = 0xD1D1D1D1;  // edi
 616         *--stack = 0x54545454;  // esp - NOT POPED
 617         *--stack = 0x51515151;  // esi
 618         *--stack = 0xB4B4B4B4;  // ebp
 619         //Individual PUSHs
 620         *--stack = SS;  // ds
 621         *--stack = SS;  // es
 622         *--stack = SS;  // fs
 623         *--stack = SS;  // gs
 624
 625         __asm__ __volatile__ (
 626         "mov %%eax,%%esp;\n\t"  // Set stack pointer
 627         "pop %%gs;\n\t"
 628         "pop %%fs;\n\t"
 629         "pop %%es;\n\t"
 630         "pop %%ds;\n\t"
 631         "popa;\n\t"
 632         "iret;\n\t" : : "a" (stack));
 633         for(;;);
 634 }
 635
 636 /**
 637  * \brief Calls a signal handler in user mode
 638  * \note Used for signals
 639  */
 640 void Proc_CallFaultHandler(tThread *Thread)
 641 {
 642         // Rewinds the stack and calls the user function
 643         // Never returns
 644         Proc_ReturnToUser( Thread->FaultHandler, Thread->CurFaultNum, Thread->KernelStack );
 645         for(;;);
 646 }
 647
 648 void Proc_DumpThreadCPUState(tThread *Thread)
 649 {
 650         if( Thread->CurCPU > -1 )
 651         {
 652                  int    maxBacktraceDistance = 6;
 653                 tRegs   *regs = NULL;
 654                 Uint32  *stack;
 655
 656                 if( Thread->CurCPU != GetCPUNum() ) {
 657                         Log("  Currently running");
 658                         return ;
 659                 }
 660
 661                 // Backtrace to find the IRQ entrypoint
 662                 // - This will usually only be called by an IRQ, so this should
 663                 //   work
 664                 __asm__ __volatile__ ("mov %%ebp, %0" : "=r" (stack));
 665                 while( maxBacktraceDistance -- )
 666                 {
 667                         if( !CheckMem(stack, 8) ) {
 668                                 regs = NULL;
 669                                 break;
 670                         }
 671                         // [ebp] = oldEbp
 672                         // [ebp+4] = retaddr
 673
 674                         if( stack[1] == (tVAddr)&IRQCommon_handled ) {
 675                                 regs = (void*)stack[2];
 676                                 break;
 677                         }
 678
 679                         stack = (void*)stack[0];
 680                 }
 681
 682                 if( !regs ) {
 683                         Log("  Unable to find IRQ Entry");
 684                         return ;
 685                 }
 686
 687                 Log("  at %04x:%08x", regs->cs, regs->eip);
 688                 return ;
 689         }
 690
 691         tVAddr  diffFromScheduler = Thread->SavedState.EIP - (tVAddr)SwitchTasks;
 692         tVAddr  diffFromClone = Thread->SavedState.EIP - (tVAddr)Proc_CloneInt;
 693         tVAddr  diffFromSpawn = Thread->SavedState.EIP - (tVAddr)NewTaskHeader;
 694
 695         if( diffFromClone > 0 && diffFromClone < 40 )   // When I last checked, .newTask was at .+27
 696         {
 697                 Log("  Creating process");
 698                 return ;
 699         }
 700
 701         if( diffFromSpawn == 0 )
 702         {
 703                 Log("  Creating thread");
 704                 return ;
 705         }
 706
 707         if( diffFromScheduler > 0 && diffFromScheduler < 128 )  // When I last checked, GetEIP was at .+0x30
 708         {
 709                 // Scheduled out
 710                 Log("  At %04x:%08x", Thread->SavedState.UserCS, Thread->SavedState.UserEIP);
 711                 return ;
 712         }
 713
 714         Log("  Just created (unknown %p)", Thread->SavedState.EIP);
 715 }
 716
 717 void Proc_Reschedule(void)
 718 {
 719         tThread *nextthread, *curthread;
 720          int    cpu = GetCPUNum();
 721
 722         // TODO: Wait for the lock?
 723         if(IS_LOCKED(&glThreadListLock))        return;
 724
 725         curthread = Proc_GetCurThread();
 726
 727         nextthread = Threads_GetNextToRun(cpu, curthread);
 728
 729         if(!nextthread || nextthread == curthread)
 730                 return ;
 731
 732         #if DEBUG_TRACE_SWITCH
 733         // HACK: Ignores switches to the idle threads
 734         if( nextthread->TID == 0 || nextthread->TID > giNumCPUs )
 735         {
 736                 LogF("\nSwitching CPU %i to %p (%i %s) - CR3 = 0x%x, EIP = %p, ESP = %p\n",
 737                         GetCPUNum(),
 738                         nextthread, nextthread->TID, nextthread->ThreadName,
 739                         nextthread->Process->MemState.CR3,
 740                         nextthread->SavedState.EIP,
 741                         nextthread->SavedState.ESP
 742                         );
 743                 LogF("OldCR3 = %P\n", curthread->Process->MemState.CR3);
 744         }
 745         #endif
 746
 747         // Update CPU state
 748         gaCPUs[cpu].Current = nextthread;
 749         gaCPUs[cpu].LastTimerThread = NULL;
 750         gTSSs[cpu].ESP0 = nextthread->KernelStack-4;
 751         __asm__ __volatile__("mov %0, %%db0\n\t" : : "r"(nextthread) );
 752
 753         // Save FPU/MMX/XMM/SSE state
 754         if( curthread && curthread->SavedState.SSE )
 755         {
 756                 Proc_SaveSSE( ((Uint)curthread->SavedState.SSE + 0xF) & ~0xF );
 757                 curthread->SavedState.bSSEModified = 0;
 758                 Proc_DisableSSE();
 759         }
 760
 761         if( curthread )
 762         {
 763                 SwitchTasks(
 764                         nextthread->SavedState.ESP, &curthread->SavedState.ESP,
 765                         nextthread->SavedState.EIP, &curthread->SavedState.EIP,
 766                         nextthread->Process->MemState.CR3
 767                         );
 768         }
 769         else
 770         {
 771                 SwitchTasks(
 772                         nextthread->SavedState.ESP, 0,
 773                         nextthread->SavedState.EIP, 0,
 774                         nextthread->Process->MemState.CR3
 775                         );
 776         }
 777
 778         return ;
 779 }
 780
 781 /**
 782  * \fn void Proc_Scheduler(int CPU)
 783  * \brief Swap current thread and clears dead threads
 784  */
 785 void Proc_Scheduler(int CPU)
 786 {
 787         #if USE_MP
 788         if( GetCPUNum() )
 789                 gpMP_LocalAPIC->EOI.Val = 0;
 790         else
 791         #endif
 792                 outb(0x20, 0x20);
 793         __asm__ __volatile__ ("sti");
 794
 795         // Call the timer update code
 796         Timer_CallTimers();
 797
 798         #if !DEBUG_NOPREEMPT
 799         // If two ticks happen within the same task, and it's not an idle task, swap
 800         if( gaCPUs[CPU].Current->TID > giNumCPUs && gaCPUs[CPU].Current == gaCPUs[CPU].LastTimerThread )
 801         {
 802                 Proc_Reschedule();
 803         }
 804
 805         gaCPUs[CPU].LastTimerThread = gaCPUs[CPU].Current;
 806         #endif
 807 }
 808
 809 // === EXPORTS ===
 810 EXPORT(Proc_SpawnWorker);