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