KernelLand/Kernel/arch/x86_64/proc.c

   1 /*
   2  * Acess2 x86_64 port
   3  * proc.c
   4  */
   5 #include <acess.h>
   6 #include <proc.h>
   7 #include <threads.h>
   8 #include <threads_int.h>
   9 #include <desctab.h>
  10 #include <mm_virt.h>
  11 #include <errno.h>
  12 #if USE_MP
  13 # include <mp.h>
  14 #endif
  15 #include <arch_config.h>
  16 #include <hal_proc.h>
  17
  18 // === FLAGS ===
  19 #define DEBUG_TRACE_SWITCH      0
  20 #define BREAK_ON_SWITCH         0       // Break into bochs debugger on a task switch
  21
  22 // === CONSTANTS ===
  23
  24 // === TYPES ===
  25 typedef struct sCPU
  26 {
  27         Uint8   APICID;
  28         Uint8   State;  // 0: Unavaliable, 1: Idle, 2: Active
  29         Uint16  Resvd;
  30         tThread *Current;
  31         tThread *IdleThread;
  32 }       tCPU;
  33
  34 // === IMPORTS ===
  35 extern tGDT     gGDT[];
  36 extern void     APStartup(void);        // 16-bit AP startup code
  37
  38 extern Uint     GetRIP(void);   // start.asm
  39 extern Uint     SaveState(Uint *RSP, Uint *Regs);
  40 extern Uint     Proc_CloneInt(Uint *RSP, Uint *CR3);
  41 extern void     NewTaskHeader(void);    // Actually takes cdecl args
  42 extern void     Proc_InitialiseSSE(void);
  43 extern void     Proc_SaveSSE(Uint DestPtr);
  44 extern void     Proc_DisableSSE(void);
  45
  46 extern Uint64   gInitialPML4[512];      // start.asm
  47 extern int      giNumCPUs;
  48 extern int      giNextTID;
  49 extern int      giTotalTickets;
  50 extern int      giNumActiveThreads;
  51 extern tThread  gThreadZero;
  52 extern tProcess gProcessZero;
  53 extern void     Threads_Dump(void);
  54 extern void     Proc_ReturnToUser(tVAddr Handler, tVAddr KStackTop, int Argument);
  55 extern void     Time_UpdateTimestamp(void);
  56 extern void     SwitchTasks(Uint NewSP, Uint *OldSP, Uint NewIP, Uint *OldIO, Uint CR3);
  57
  58 // === PROTOTYPES ===
  59 //void  ArchThreads_Init(void);
  60 #if USE_MP
  61 void    MP_StartAP(int CPU);
  62 void    MP_SendIPI(Uint8 APICID, int Vector, int DeliveryMode);
  63 #endif
  64 void    Proc_IdleTask(void *unused);
  65 //void  Proc_Start(void);
  66 //tThread       *Proc_GetCurThread(void);
  67 // int  Proc_NewKThread(void (*Fcn)(void*), void *Data);
  68 // int  Proc_Clone(Uint *Err, Uint Flags);
  69 // int  Proc_SpawnWorker(void);
  70 Uint    Proc_MakeUserStack(void);
  71 //void  Proc_StartUser(Uint Entrypoint, Uint *Bases, int ArgC, char **ArgV, char **EnvP, int DataSize);
  72 void    Proc_StartProcess(Uint16 SS, Uint Stack, Uint Flags, Uint16 CS, Uint IP) NORETURN;
  73  int    Proc_Demote(Uint *Err, int Dest, tRegs *Regs);
  74 //void  Proc_CallFaultHandler(tThread *Thread);
  75 //void  Proc_DumpThreadCPUState(tThread *Thread);
  76 //void  Proc_Reschedule(void);
  77 void    Proc_Scheduler(int CPU, Uint RSP, Uint RIP);
  78
  79 // === GLOBALS ===
  80 //!\brief Used by desctab.asm in SyscallStub
  81 const int ci_offsetof_tThread_KernelStack = offsetof(tThread, KernelStack);
  82 // --- Multiprocessing ---
  83 #if USE_MP
  84 volatile int    giNumInitingCPUs = 0;
  85 tMPInfo *gMPFloatPtr = NULL;
  86 tAPIC   *gpMP_LocalAPIC = NULL;
  87 Uint8   gaAPIC_to_CPU[256] = {0};
  88 #endif
  89 tCPU    gaCPUs[MAX_CPUS];
  90 tTSS    *gTSSs = NULL;
  91 tTSS    gTSS0 = {0};
  92 // --- Error Recovery ---
  93 Uint32  gaDoubleFaultStack[1024];
  94
  95 // === CODE ===
  96 /**
  97  * \fn void ArchThreads_Init(void)
  98  * \brief Starts the process scheduler
  99  */
 100 void ArchThreads_Init(void)
 101 {
 102         Uint    pos = 0;
 103
 104         #if USE_MP
 105         tMPTable        *mptable;
 106
 107         // Mark BSP as active
 108         gaCPUs[0].State = 2;
 109
 110         // -- Initialise Multiprocessing
 111         // Find MP Floating Table
 112         // - EBDA/Last 1Kib (640KiB)
 113         for(pos = KERNEL_BASE|0x9F000; pos < (KERNEL_BASE|0xA0000); pos += 16) {
 114                 if( *(Uint*)(pos) == MPPTR_IDENT ) {
 115                         Log("Possible %p", pos);
 116                         if( ByteSum((void*)pos, sizeof(tMPInfo)) != 0 ) continue;
 117                         gMPFloatPtr = (void*)pos;
 118                         break;
 119                 }
 120         }
 121         // - Last KiB (512KiB base mem)
 122         if(!gMPFloatPtr) {
 123                 for(pos = KERNEL_BASE|0x7F000; pos < (KERNEL_BASE|0x80000); pos += 16) {
 124                         if( *(Uint*)(pos) == MPPTR_IDENT ) {
 125                                 Log("Possible %p", pos);
 126                                 if( ByteSum((void*)pos, sizeof(tMPInfo)) != 0 ) continue;
 127                                 gMPFloatPtr = (void*)pos;
 128                                 break;
 129                         }
 130                 }
 131         }
 132         // - BIOS ROM
 133         if(!gMPFloatPtr) {
 134                 for(pos = KERNEL_BASE|0xE0000; pos < (KERNEL_BASE|0x100000); pos += 16) {
 135                         if( *(Uint*)(pos) == MPPTR_IDENT ) {
 136                                 Log("Possible %p", pos);
 137                                 if( ByteSum((void*)pos, sizeof(tMPInfo)) != 0 ) continue;
 138                                 gMPFloatPtr = (void*)pos;
 139                                 break;
 140                         }
 141                 }
 142         }
 143
 144         // If the MP Table Exists, parse it
 145         if(gMPFloatPtr)
 146         {
 147                  int    i;
 148                 tMPTable_Ent    *ents;
 149                 Log("gMPFloatPtr = %p", gMPFloatPtr);
 150                 Log("*gMPFloatPtr = {");
 151                 Log("\t.Sig = 0x%08x", gMPFloatPtr->Sig);
 152                 Log("\t.MPConfig = 0x%08x", gMPFloatPtr->MPConfig);
 153                 Log("\t.Length = 0x%02x", gMPFloatPtr->Length);
 154                 Log("\t.Version = 0x%02x", gMPFloatPtr->Version);
 155                 Log("\t.Checksum = 0x%02x", gMPFloatPtr->Checksum);
 156                 Log("\t.Features = [0x%02x,0x%02x,0x%02x,0x%02x,0x%02x]",
 157                         gMPFloatPtr->Features[0],       gMPFloatPtr->Features[1],
 158                         gMPFloatPtr->Features[2],       gMPFloatPtr->Features[3],
 159                         gMPFloatPtr->Features[4]
 160                         );
 161                 Log("}");
 162
 163                 mptable = (void*)( KERNEL_BASE|gMPFloatPtr->MPConfig );
 164                 Log("mptable = %p", mptable);
 165                 Log("*mptable = {");
 166                 Log("\t.Sig = 0x%08x", mptable->Sig);
 167                 Log("\t.BaseTableLength = 0x%04x", mptable->BaseTableLength);
 168                 Log("\t.SpecRev = 0x%02x", mptable->SpecRev);
 169                 Log("\t.Checksum = 0x%02x", mptable->Checksum);
 170                 Log("\t.OEMID = '%8c'", mptable->OemID);
 171                 Log("\t.ProductID = '%8c'", mptable->ProductID);
 172                 Log("\t.OEMTablePtr = %p'", mptable->OEMTablePtr);
 173                 Log("\t.OEMTableSize = 0x%04x", mptable->OEMTableSize);
 174                 Log("\t.EntryCount = 0x%04x", mptable->EntryCount);
 175                 Log("\t.LocalAPICMemMap = 0x%08x", mptable->LocalAPICMemMap);
 176                 Log("\t.ExtendedTableLen = 0x%04x", mptable->ExtendedTableLen);
 177                 Log("\t.ExtendedTableChecksum = 0x%02x", mptable->ExtendedTableChecksum);
 178                 Log("}");
 179
 180                 gpMP_LocalAPIC = (void*)MM_MapHWPage(mptable->LocalAPICMemMap, 1);
 181
 182                 ents = mptable->Entries;
 183                 giNumCPUs = 0;
 184
 185                 for( i = 0; i < mptable->EntryCount; i ++ )
 186                 {
 187                          int    entSize = 0;
 188                         switch( ents->Type )
 189                         {
 190                         case 0: // Processor
 191                                 entSize = 20;
 192                                 Log("%i: Processor", i);
 193                                 Log("\t.APICID = %i", ents->Proc.APICID);
 194                                 Log("\t.APICVer = 0x%02x", ents->Proc.APICVer);
 195                                 Log("\t.CPUFlags = 0x%02x", ents->Proc.CPUFlags);
 196                                 Log("\t.CPUSignature = 0x%08x", ents->Proc.CPUSignature);
 197                                 Log("\t.FeatureFlags = 0x%08x", ents->Proc.FeatureFlags);
 198
 199
 200                                 if( !(ents->Proc.CPUFlags & 1) ) {
 201                                         Log("DISABLED");
 202                                         break;
 203                                 }
 204
 205                                 // Check if there is too many processors
 206                                 if(giNumCPUs >= MAX_CPUS) {
 207                                         giNumCPUs ++;   // If `giNumCPUs` > MAX_CPUS later, it will be clipped
 208                                         break;
 209                                 }
 210
 211                                 // Initialise CPU Info
 212                                 gaAPIC_to_CPU[ents->Proc.APICID] = giNumCPUs;
 213                                 gaCPUs[giNumCPUs].APICID = ents->Proc.APICID;
 214                                 gaCPUs[giNumCPUs].State = 0;
 215                                 giNumCPUs ++;
 216
 217                                 // Send IPI
 218                                 if( !(ents->Proc.CPUFlags & 2) )
 219                                 {
 220                                         MP_StartAP( giNumCPUs-1 );
 221                                 }
 222
 223                                 break;
 224                         case 1: // Bus
 225                                 entSize = 8;
 226                                 Log("%i: Bus", i);
 227                                 Log("\t.ID = %i", ents->Bus.ID);
 228                                 Log("\t.TypeString = '%6c'", ents->Bus.TypeString);
 229                                 break;
 230                         case 2: // I/O APIC
 231                                 entSize = 8;
 232                                 Log("%i: I/O APIC", i);
 233                                 Log("\t.ID = %i", ents->IOAPIC.ID);
 234                                 Log("\t.Version = 0x%02x", ents->IOAPIC.Version);
 235                                 Log("\t.Flags = 0x%02x", ents->IOAPIC.Flags);
 236                                 Log("\t.Addr = 0x%08x", ents->IOAPIC.Addr);
 237                                 break;
 238                         case 3: // I/O Interrupt Assignment
 239                                 entSize = 8;
 240                                 Log("%i: I/O Interrupt Assignment", i);
 241                                 Log("\t.IntType = %i", ents->IOInt.IntType);
 242                                 Log("\t.Flags = 0x%04x", ents->IOInt.Flags);
 243                                 Log("\t.SourceBusID = 0x%02x", ents->IOInt.SourceBusID);
 244                                 Log("\t.SourceBusIRQ = 0x%02x", ents->IOInt.SourceBusIRQ);
 245                                 Log("\t.DestAPICID = 0x%02x", ents->IOInt.DestAPICID);
 246                                 Log("\t.DestAPICIRQ = 0x%02x", ents->IOInt.DestAPICIRQ);
 247                                 break;
 248                         case 4: // Local Interrupt Assignment
 249                                 entSize = 8;
 250                                 Log("%i: Local Interrupt Assignment", i);
 251                                 Log("\t.IntType = %i", ents->LocalInt.IntType);
 252                                 Log("\t.Flags = 0x%04x", ents->LocalInt.Flags);
 253                                 Log("\t.SourceBusID = 0x%02x", ents->LocalInt.SourceBusID);
 254                                 Log("\t.SourceBusIRQ = 0x%02x", ents->LocalInt.SourceBusIRQ);
 255                                 Log("\t.DestLocalAPICID = 0x%02x", ents->LocalInt.DestLocalAPICID);
 256                                 Log("\t.DestLocalAPICIRQ = 0x%02x", ents->LocalInt.DestLocalAPICIRQ);
 257                                 break;
 258                         default:
 259                                 Log("%i: Unknown (%i)", i, ents->Type);
 260                                 break;
 261                         }
 262                         ents = (void*)( (Uint)ents + entSize );
 263                 }
 264
 265                 if( giNumCPUs > MAX_CPUS ) {
 266                         Warning("Too many CPUs detected (%i), only using %i of them", giNumCPUs, MAX_CPUS);
 267                         giNumCPUs = MAX_CPUS;
 268                 }
 269
 270                 while( giNumInitingCPUs )
 271                         MM_FinishVirtualInit();
 272
 273                 Panic("Uh oh... MP Table Parsing is unimplemented\n");
 274         }
 275         else {
 276                 Log("No MP Table was found, assuming uniprocessor\n");
 277                 giNumCPUs = 1;
 278                 gTSSs = &gTSS0;
 279         }
 280         #else
 281         giNumCPUs = 1;
 282         gTSSs = &gTSS0;
 283         MM_FinishVirtualInit();
 284         #endif
 285
 286         #if USE_MP
 287         // Initialise Normal TSS(s)
 288         for(pos=0;pos<giNumCPUs;pos++)
 289         {
 290         #else
 291         pos = 0;
 292         #endif
 293                 gTSSs[pos].RSP0 = 0;    // Set properly by scheduler
 294                 gGDT[7+pos*2].LimitLow = sizeof(tTSS) & 0xFFFF;
 295                 gGDT[7+pos*2].BaseLow = ((Uint)(&gTSSs[pos])) & 0xFFFF;
 296                 gGDT[7+pos*2].BaseMid = ((Uint)(&gTSSs[pos])) >> 16;
 297                 gGDT[7+pos*2].BaseHi = ((Uint)(&gTSSs[pos])) >> 24;
 298                 gGDT[7+pos*2+1].DWord[0] = ((Uint)(&gTSSs[pos])) >> 32;
 299         #if USE_MP
 300         }
 301         for(pos=0;pos<giNumCPUs;pos++) {
 302                 __asm__ __volatile__ ("ltr %%ax"::"a"(0x38+pos*16));
 303         }
 304         #else
 305         __asm__ __volatile__ ("ltr %%ax"::"a"(0x38));
 306         #endif
 307
 308         // Set Debug registers
 309         __asm__ __volatile__ ("mov %0, %%db0" : : "r"(&gThreadZero));
 310         __asm__ __volatile__ ("mov %%rax, %%db1" : : "a"(0));
 311
 312         gaCPUs[0].Current = &gThreadZero;
 313
 314         gProcessZero.MemState.CR3 = (Uint)gInitialPML4 - KERNEL_BASE;
 315         gThreadZero.CurCPU = 0;
 316         gThreadZero.KernelStack = 0xFFFFA00000000000 + KERNEL_STACK_SIZE;
 317
 318         // Set timer frequency
 319         outb(0x43, 0x34);       // Set Channel 0, Low/High, Rate Generator
 320         outb(0x40, PIT_TIMER_DIVISOR&0xFF);     // Low Byte of Divisor
 321         outb(0x40, (PIT_TIMER_DIVISOR>>8)&0xFF);        // High Byte
 322
 323         // Create Per-Process Data Block
 324         if( !MM_Allocate(MM_PPD_CFG) )
 325         {
 326                 Warning("Oh, hell, Unable to allocate PPD for Thread#0");
 327         }
 328
 329         Proc_InitialiseSSE();
 330
 331         Log_Log("Proc", "Multithreading initialised");
 332 }
 333
 334 #if USE_MP
 335 void MP_StartAP(int CPU)
 336 {
 337         Log("Starting AP %i (APIC %i)", CPU, gaCPUs[CPU].APICID);
 338         // Set location of AP startup code and mark for a warm restart
 339         *(Uint16*)(KERNEL_BASE|0x467) = (Uint)&APStartup - (KERNEL_BASE|0xFFFF0);
 340         *(Uint16*)(KERNEL_BASE|0x469) = 0xFFFF;
 341         outb(0x70, 0x0F);       outb(0x71, 0x0A);       // Warm Reset
 342         MP_SendIPI(gaCPUs[CPU].APICID, 0, 5);
 343         giNumInitingCPUs ++;
 344 }
 345
 346 void MP_SendIPI(Uint8 APICID, int Vector, int DeliveryMode)
 347 {
 348         Uint32  addr = (Uint)gpMP_LocalAPIC + 0x300;
 349         Uint32  val;
 350
 351         // High
 352         val = (Uint)APICID << 24;
 353         Log("*%p = 0x%08x", addr+0x10, val);
 354         *(Uint32*)(addr+0x10) = val;
 355         // Low (and send)
 356         val = ((DeliveryMode & 7) << 8) | (Vector & 0xFF);
 357         Log("*%p = 0x%08x", addr, val);
 358         *(Uint32*)addr = val;
 359 }
 360 #endif
 361
 362 /**
 363  * \brief Idle task
 364  */
 365 void Proc_IdleTask(void *ptr)
 366 {
 367         tCPU    *cpu = ptr;
 368         cpu->IdleThread = Proc_GetCurThread();
 369         cpu->IdleThread->ThreadName = (char*)"Idle Thread";
 370         Threads_SetPriority( cpu->IdleThread, -1 );     // Never called randomly
 371         cpu->IdleThread->Quantum = 1;   // 1 slice quantum
 372         for(;;) {
 373                 HALT(); // Just yeilds
 374                 Threads_Yield();
 375         }
 376 }
 377
 378 /**
 379  * \fn void Proc_Start(void)
 380  * \brief Start process scheduler
 381  */
 382 void Proc_Start(void)
 383 {
 384         #if USE_MP
 385          int    i;
 386         #endif
 387
 388         #if USE_MP
 389         // Start APs
 390         for( i = 0; i < giNumCPUs; i ++ )
 391         {
 392                  int    tid;
 393                 if(i)   gaCPUs[i].Current = NULL;
 394
 395                 Proc_NewKThread(Proc_IdleTask, &gaCPUs[i]);
 396
 397                 // Create Idle Task
 398                 gaCPUs[i].IdleThread = Threads_GetThread(tid);
 399
 400
 401                 // Start the AP
 402                 if( i != giProc_BootProcessorID ) {
 403                         MP_StartAP( i );
 404                 }
 405         }
 406
 407         // BSP still should run the current task
 408         gaCPUs[0].Current = &gThreadZero;
 409         __asm__ __volatile__ ("mov %0, %%db0" : : "r"(&gThreadZero));
 410
 411         // Start interrupts and wait for APs to come up
 412         Log("Waiting for APs to come up\n");
 413         __asm__ __volatile__ ("sti");
 414         while( giNumInitingCPUs )       __asm__ __volatile__ ("hlt");
 415         #else
 416         Proc_NewKThread(Proc_IdleTask, &gaCPUs[0]);
 417
 418         // Start Interrupts (and hence scheduler)
 419         __asm__ __volatile__("sti");
 420         #endif
 421         MM_FinishVirtualInit();
 422         Log_Log("Proc", "Multithreading started");
 423 }
 424
 425 /**
 426  * \fn tThread *Proc_GetCurThread(void)
 427  * \brief Gets the current thread
 428  */
 429 tThread *Proc_GetCurThread(void)
 430 {
 431         #if USE_MP
 432         tThread *ret;
 433         __asm__ __volatile__ ("mov %%db0, %0" : "=r"(thread));
 434         return ret;     // gaCPUs[ GetCPUNum() ].Current;
 435         #else
 436         return gaCPUs[ 0 ].Current;
 437         #endif
 438 }
 439
 440 void Proc_ClearProcess(tProcess *Process)
 441 {
 442         Log_Warning("Proc", "TODO: Nuke address space etc");
 443 }
 444
 445 /*
 446  *
 447  */
 448 void Proc_ClearThread(tThread *Thread)
 449 {
 450 }
 451
 452 /**
 453  * \brief Create a new kernel thread
 454  */
 455 tTID Proc_NewKThread(void (*Fcn)(void*), void *Data)
 456 {
 457         Uint    rsp;
 458         tThread *newThread, *cur;
 459
 460         cur = Proc_GetCurThread();
 461         newThread = Threads_CloneTCB(0);
 462         if(!newThread)  return -1;
 463
 464         // Create new KStack
 465         newThread->KernelStack = MM_NewKStack();
 466         // Check for errors
 467         if(newThread->KernelStack == 0) {
 468                 free(newThread);
 469                 return -1;
 470         }
 471
 472         rsp = newThread->KernelStack;
 473         *(Uint*)(rsp-=8) = (Uint)Data;  // Data (shadowed)
 474         *(Uint*)(rsp-=8) = (Uint)Fcn;   // Function to call
 475         *(Uint*)(rsp-=8) = (Uint)newThread;     // Thread ID
 476
 477         newThread->SavedState.RSP = rsp;
 478         newThread->SavedState.RIP = (Uint)&NewTaskHeader;
 479         newThread->SavedState.SSE = NULL;
 480 //      Log("New (KThread) %p, rsp = %p\n", newThread->SavedState.RIP, newThread->SavedState.RSP);
 481
 482 //      MAGIC_BREAK();
 483         Threads_AddActive(newThread);
 484
 485         return newThread->TID;
 486 }
 487
 488 /**
 489  * \fn int Proc_Clone(Uint Flags)
 490  * \brief Clone the current process
 491  */
 492 tTID Proc_Clone(Uint Flags)
 493 {
 494         tThread *newThread, *cur = Proc_GetCurThread();
 495         Uint    rip;
 496
 497         // Sanity check
 498         if( !(Flags & CLONE_VM) ) {
 499                 Log_Error("Proc", "Proc_Clone: Don't leave CLONE_VM unset, use Proc_NewKThread instead");
 500                 return -1;
 501         }
 502
 503         // Create new TCB
 504         newThread = Threads_CloneTCB(Flags);
 505         if(!newThread)  return -1;
 506
 507         // Save core machine state
 508         rip = Proc_CloneInt(&newThread->SavedState.RSP, &newThread->Process->MemState.CR3);
 509         if(rip == 0)    return 0;       // Child
 510         newThread->KernelStack = cur->KernelStack;
 511         newThread->SavedState.RIP = rip;
 512         newThread->SavedState.SSE = NULL;
 513
 514         // DEBUG
 515         #if 0
 516         Log("New (Clone) %p, rsp = %p, cr3 = %p", rip, newThread->SavedState.RSP, newThread->MemState.CR3);
 517         {
 518                 Uint cr3;
 519                 __asm__ __volatile__ ("mov %%cr3, %0" : "=r" (cr3));
 520                 Log("Current CR3 = 0x%x, PADDR(RSP) = 0x%x", cr3, MM_GetPhysAddr(newThread->SavedState.RSP));
 521         }
 522         #endif
 523         // /DEBUG
 524
 525         // Lock list and add to active
 526         Threads_AddActive(newThread);
 527
 528         return newThread->TID;
 529 }
 530
 531 /**
 532  * \fn int Proc_SpawnWorker(void)
 533  * \brief Spawns a new worker thread
 534  */
 535 int Proc_SpawnWorker(void (*Fcn)(void*), void *Data)
 536 {
 537         tThread *new, *cur;
 538         Uint    stack_contents[3];
 539
 540         cur = Proc_GetCurThread();
 541
 542         // Create new thread
 543         new = Threads_CloneThreadZero();
 544         if(!new) {
 545                 Warning("Proc_SpawnWorker - Out of heap space!\n");
 546                 return -1;
 547         }
 548
 549         // Create the stack contents
 550         stack_contents[2] = (Uint)Data;
 551         stack_contents[1] = (Uint)Fcn;
 552         stack_contents[0] = (Uint)new;
 553
 554         // Create a new worker stack (in PID0's address space)
 555         // - The stack is built by this code using stack_contents
 556         new->KernelStack = MM_NewWorkerStack(stack_contents, sizeof(stack_contents));
 557
 558         new->SavedState.RSP = new->KernelStack - sizeof(stack_contents);
 559         new->SavedState.RIP = (Uint)&NewTaskHeader;
 560         new->SavedState.SSE = NULL;
 561
 562 //      Log("New (Worker) %p, rsp = %p\n", new->SavedState.RIP, new->SavedState.RSP);
 563
 564         // Mark as active
 565         new->Status = THREAD_STAT_PREINIT;
 566         Threads_AddActive( new );
 567
 568         return new->TID;
 569 }
 570
 571 /**
 572  * \brief Creates a new user stack
 573  */
 574 Uint Proc_MakeUserStack(void)
 575 {
 576          int    i;
 577         Uint    base = USER_STACK_TOP - USER_STACK_SZ;
 578
 579         // Check Prospective Space
 580         for( i = USER_STACK_SZ >> 12; i--; )
 581         {
 582                 if( MM_GetPhysAddr( (void*)(base + (i<<12)) ) != 0 )
 583                         break;
 584         }
 585
 586         if(i != -1)     return 0;
 587
 588         // Allocate Stack - Allocate incrementally to clean up MM_Dump output
 589         // - Most of the user stack is the zero page
 590         for( i = 0; i < (USER_STACK_SZ-USER_STACK_PREALLOC)/0x1000; i++ )
 591         {
 592                 MM_AllocateZero( base + (i<<12) );
 593         }
 594         // - but the top USER_STACK_PREALLOC pages are actually allocated
 595         for( ; i < USER_STACK_SZ/0x1000; i++ )
 596         {
 597                 tPAddr  alloc = MM_Allocate( base + (i<<12) );
 598                 if( !alloc )
 599                 {
 600                         // Error
 601                         Log_Error("Proc", "Unable to allocate user stack (%i pages requested)", USER_STACK_SZ/0x1000);
 602                         while( i -- )
 603                                 MM_Deallocate( base + (i<<12) );
 604                         return 0;
 605                 }
 606         }
 607
 608         return base + USER_STACK_SZ;
 609 }
 610
 611 void Proc_StartUser(Uint Entrypoint, Uint Base, int ArgC, const char **ArgV, int DataSize)
 612 {
 613         Uint    *stack;
 614          int    i;
 615         const char      **envp = NULL;
 616         Uint16  ss, cs;
 617
 618
 619         // Copy Arguments
 620         stack = (void*)Proc_MakeUserStack();
 621         if(!stack) {
 622                 Log_Error("Proc", "Unable to create user stack!");
 623                 Threads_Exit(0, -1);
 624         }
 625         stack -= (DataSize+7)/8;
 626         memcpy( stack, ArgV, DataSize );
 627         free(ArgV);
 628
 629         // Adjust Arguments and environment
 630         if(DataSize)
 631         {
 632                 Uint    delta = (Uint)stack - (Uint)ArgV;
 633                 ArgV = (const char**)stack;
 634                 for( i = 0; ArgV[i]; i++ )      ArgV[i] += delta;
 635                 envp = &ArgV[i+1];
 636                 for( i = 0; envp[i]; i++ )      envp[i] += delta;
 637         }
 638
 639         // User Mode Segments
 640         // 0x2B = 64-bit
 641         ss = 0x23;      cs = 0x2B;
 642
 643         // Arguments
 644         *--stack = (Uint)envp;
 645         *--stack = (Uint)ArgV;
 646         *--stack = (Uint)ArgC;
 647         *--stack = Base;
 648
 649         Proc_StartProcess(ss, (Uint)stack, 0x202, cs, Entrypoint);
 650 }
 651
 652 void Proc_StartProcess(Uint16 SS, Uint Stack, Uint Flags, Uint16 CS, Uint IP)
 653 {
 654         if( !(CS == 0x1B || CS == 0x2B) || SS != 0x23 ) {
 655                 Log_Error("Proc", "Proc_StartProcess: CS / SS are not valid (%x, %x)",
 656                         CS, SS);
 657                 Threads_Exit(0, -1);
 658         }
 659 //      Log("Proc_StartProcess: (SS=%x, Stack=%p, Flags=%x, CS=%x, IP=%p)", SS, Stack, Flags, CS, IP);
 660 //      MM_DumpTables(0, USER_MAX);
 661         if(CS == 0x1B)
 662         {
 663                 // 32-bit return
 664                 __asm__ __volatile__ (
 665                         "mov %0, %%rsp;\n\t"    // Set stack pointer
 666                         "mov %2, %%r11;\n\t"    // Set RFLAGS
 667                         "sysret;\n\t"
 668                         : : "r" (Stack), "c" (IP), "r" (Flags)
 669                         );
 670         }
 671         else
 672         {
 673                 // 64-bit return
 674                 __asm__ __volatile__ (
 675                         "mov %0, %%rsp;\n\t"    // Set stack pointer
 676                         "mov %2, %%r11;\n\t"    // Set RFLAGS
 677                         "sysretq;\n\t"
 678                         : : "r" (Stack), "c" (IP), "r" (Flags)
 679                         );
 680         }
 681         for(;;);
 682 }
 683
 684 /**
 685  * \fn int Proc_Demote(Uint *Err, int Dest, tRegs *Regs)
 686  * \brief Demotes a process to a lower permission level
 687  * \param Err   Pointer to user's errno
 688  * \param Dest  New Permission Level
 689  * \param Regs  Pointer to user's register structure
 690  */
 691 int Proc_Demote(Uint *Err, int Dest, tRegs *Regs)
 692 {
 693          int    cpl = Regs->CS & 3;
 694         // Sanity Check
 695         if(Dest > 3 || Dest < 0) {
 696                 *Err = -EINVAL;
 697                 return -1;
 698         }
 699
 700         // Permission Check
 701         if(cpl > Dest) {
 702                 *Err = -EACCES;
 703                 return -1;
 704         }
 705
 706         // Change the Segment Registers
 707         Regs->CS = (((Dest+1)<<4) | Dest) - 8;
 708         Regs->SS = ((Dest+1)<<4) | Dest;
 709
 710         return 0;
 711 }
 712
 713 /**
 714  * \brief Calls a signal handler in user mode
 715  * \note Used for signals
 716  */
 717 void Proc_CallFaultHandler(tThread *Thread)
 718 {
 719         // Never returns
 720         Proc_ReturnToUser(Thread->FaultHandler, Thread->KernelStack, Thread->CurFaultNum);
 721         for(;;);
 722 }
 723
 724 void Proc_DumpThreadCPUState(tThread *Thread)
 725 {
 726         Log("  At %04x:%016llx", Thread->SavedState.UserCS, Thread->SavedState.UserRIP);
 727 }
 728
 729 void Proc_Reschedule(void)
 730 {
 731         tThread *nextthread, *curthread;
 732          int    cpu = GetCPUNum();
 733
 734         // TODO: Wait for it?
 735         if(IS_LOCKED(&glThreadListLock))        return;
 736
 737         curthread = gaCPUs[cpu].Current;
 738
 739         nextthread = Threads_GetNextToRun(cpu, curthread);
 740
 741         if(nextthread == curthread)     return ;
 742         if(!nextthread)
 743                 nextthread = gaCPUs[cpu].IdleThread;
 744         if(!nextthread)
 745                 return ;
 746
 747         #if DEBUG_TRACE_SWITCH
 748         LogF("\nSwitching to task CR3 = 0x%x, RIP = %p, RSP = %p - %i (%s)\n",
 749                 nextthread->Process->MemState.CR3,
 750                 nextthread->SavedState.RIP,
 751                 nextthread->SavedState.RSP,
 752                 nextthread->TID,
 753                 nextthread->ThreadName
 754                 );
 755         #endif
 756
 757         // Update CPU state
 758         gaCPUs[cpu].Current = nextthread;
 759         gTSSs[cpu].RSP0 = nextthread->KernelStack-4;
 760         __asm__ __volatile__ ("mov %0, %%db0" : : "r" (nextthread));
 761
 762         if( curthread )
 763         {
 764                 // Save FPU/MMX/XMM/SSE state
 765                 if( curthread->SavedState.SSE )
 766                 {
 767                         Proc_SaveSSE( ((Uint)curthread->SavedState.SSE + 0xF) & ~0xF );
 768                         curthread->SavedState.bSSEModified = 0;
 769                         Proc_DisableSSE();
 770                 }
 771                 SwitchTasks(
 772                         nextthread->SavedState.RSP, &curthread->SavedState.RSP,
 773                         nextthread->SavedState.RIP, &curthread->SavedState.RIP,
 774                         nextthread->Process->MemState.CR3
 775                         );
 776         }
 777         else
 778         {
 779                 Uint    tmp;
 780                 SwitchTasks(
 781                         nextthread->SavedState.RSP, &tmp,
 782                         nextthread->SavedState.RIP, &tmp,
 783                         nextthread->Process->MemState.CR3
 784                         );
 785         }
 786         return ;
 787 }
 788
 789 /**
 790  * \fn void Proc_Scheduler(int CPU)
 791  * \brief Swap current thread and clears dead threads
 792  */
 793 void Proc_Scheduler(int CPU, Uint RSP, Uint RIP)
 794 {
 795 #if 0
 796         tThread *thread;
 797
 798         // If the spinlock is set, let it complete
 799         if(IS_LOCKED(&glThreadListLock))        return;
 800
 801         // Get current thread
 802         thread = gaCPUs[CPU].Current;
 803
 804         if( thread )
 805         {
 806                 tRegs   *regs;
 807                 // Reduce remaining quantum and continue timeslice if non-zero
 808                 if(thread->Remaining--) return;
 809                 // Reset quantum for next call
 810                 thread->Remaining = thread->Quantum;
 811
 812                 // TODO: Make this more stable somehow
 813                 {
 814                         regs = (tRegs*)(RSP+(1)*8);     // CurThread
 815                         thread->SavedState.UserCS = regs->CS;
 816                         thread->SavedState.UserRIP = regs->RIP;
 817                 }
 818         }
 819
 820         // ACK Timer here?
 821
 822         Proc_Reschedule();
 823 #endif
 824 }
 825
 826 // === EXPORTS ===
 827 EXPORT(Proc_SpawnWorker);