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 #if USE_MP
  12 # include <mp.h>
  13 #endif
  14 #include <hal_proc.h>
  15
  16 // === FLAGS ===
  17 #define DEBUG_TRACE_SWITCH      1
  18 #define DEBUG_DISABLE_DOUBLEFAULT       1
  19 #define DEBUG_VERY_SLOW_SWITCH  0
  20
  21 // === CONSTANTS ===
  22 // Base is 1193182
  23 #define TIMER_BASE      1193182
  24 #if DEBUG_VERY_SLOW_PERIOD
  25 # define TIMER_DIVISOR  1193    //~10Hz switch, with 10 quantum = 1s per thread
  26 #else
  27 # define TIMER_DIVISOR  11932   //~100Hz
  28 #endif
  29
  30 // === TYPES ===
  31 typedef struct sCPU
  32 {
  33         Uint8   APICID;
  34         Uint8   State;  // 0: Unavaliable, 1: Idle, 2: Active
  35         Uint16  Resvd;
  36         tThread *Current;
  37 }       tCPU;
  38
  39 // === IMPORTS ===
  40 extern tGDT     gGDT[];
  41 extern tIDT     gIDT[];
  42 extern void     APWait(void);   // 16-bit AP pause code
  43 extern void     APStartup(void);        // 16-bit AP startup code
  44 extern Uint     GetEIP(void);   // start.asm
  45 extern Uint     GetEIP_Sched(void);     // proc.asm
  46 extern void     NewTaskHeader(tThread *Thread, void *Fcn, int nArgs, ...);      // Actually takes cdecl args
  47 extern Uint     Proc_CloneInt(Uint *ESP, Uint32 *CR3);
  48 extern Uint32   gaInitPageDir[1024];    // start.asm
  49 extern char     Kernel_Stack_Top[];
  50 extern int      giNumCPUs;
  51 extern int      giNextTID;
  52 extern tThread  gThreadZero;
  53 extern void     Isr8(void);     // Double Fault
  54 extern void     Proc_ReturnToUser(tVAddr Handler, Uint Argument, tVAddr KernelStack);
  55 extern char     scheduler_return[];     // Return address in SchedulerBase
  56 extern char     IRQCommon[];    // Common IRQ handler code
  57 extern char     IRQCommon_handled[];    // IRQCommon call return location
  58 extern char     GetEIP_Sched_ret[];     // GetEIP call return location
  59 extern void     SwitchTasks(Uint NewSP, Uint *OldSP, Uint NewIP, Uint *OldIO, Uint CR3);
  60 extern void     Proc_InitialiseSSE(void);
  61 extern void     Proc_SaveSSE(Uint DestPtr);
  62 extern void     Proc_DisableSSE(void);
  63
  64 // === PROTOTYPES ===
  65 //void  ArchThreads_Init(void);
  66 #if USE_MP
  67 void    MP_StartAP(int CPU);
  68 void    MP_SendIPIVector(int CPU, Uint8 Vector);
  69 void    MP_SendIPI(Uint8 APICID, int Vector, int DeliveryMode);
  70 #endif
  71 void    Proc_IdleThread(void *Ptr);
  72 //void  Proc_Start(void);
  73 //tThread       *Proc_GetCurThread(void);
  74 void    Proc_ChangeStack(void);
  75 // int  Proc_NewKThread(void (*Fcn)(void*), void *Data);
  76 // int  Proc_Clone(Uint *Err, Uint Flags);
  77 Uint    Proc_MakeUserStack(void);
  78 //void  Proc_StartUser(Uint Entrypoint, Uint *Bases, int ArgC, char **ArgV, char **EnvP, int DataSize);
  79 void    Proc_StartProcess(Uint16 SS, Uint Stack, Uint Flags, Uint16 CS, Uint IP) NORETURN;
  80  int    Proc_Demote(Uint *Err, int Dest, tRegs *Regs);
  81 //void  Proc_CallFaultHandler(tThread *Thread);
  82 //void  Proc_DumpThreadCPUState(tThread *Thread);
  83 void    Proc_Scheduler(int CPU);
  84
  85 // === GLOBALS ===
  86 // --- Multiprocessing ---
  87 #if USE_MP
  88 volatile int    giNumInitingCPUs = 0;
  89 tMPInfo *gMPFloatPtr = NULL;
  90 volatile Uint32 giMP_TimerCount;        // Start Count for Local APIC Timer
  91 tAPIC   *gpMP_LocalAPIC = NULL;
  92 Uint8   gaAPIC_to_CPU[256] = {0};
  93  int    giProc_BootProcessorID = 0;
  94 tTSS    gaTSSs[MAX_CPUS];       // TSS Array
  95 #endif
  96 tCPU    gaCPUs[MAX_CPUS] = {
  97         {.Current = &gThreadZero}
  98         };
  99 tTSS    *gTSSs = NULL;  // Pointer to TSS array
 100 tTSS    gTSS0 = {0};
 101 // --- Error Recovery ---
 102 char    gaDoubleFaultStack[1024] __attribute__ ((section(".padata")));
 103 tTSS    gDoubleFault_TSS = {
 104         .ESP0 = (Uint)&gaDoubleFaultStack[1024],
 105         .SS0 = 0x10,
 106         .CR3 = (Uint)gaInitPageDir - KERNEL_BASE,
 107         .EIP = (Uint)Isr8,
 108         .ESP = (Uint)&gaDoubleFaultStack[1024],
 109         .CS = 0x08,     .SS = 0x10,
 110         .DS = 0x10,     .ES = 0x10,
 111         .FS = 0x10,     .GS = 0x10,
 112 };
 113
 114 // === CODE ===
 115 /**
 116  * \fn void ArchThreads_Init(void)
 117  * \brief Starts the process scheduler
 118  */
 119 void ArchThreads_Init(void)
 120 {
 121         Uint    pos = 0;
 122
 123         #if USE_MP
 124         tMPTable        *mptable;
 125
 126         // Mark BSP as active
 127         gaCPUs[0].State = 2;
 128
 129         // -- Initialise Multiprocessing
 130         // Find MP Floating Table
 131         // - EBDA/Last 1Kib (640KiB)
 132         for(pos = KERNEL_BASE|0x9F000; pos < (KERNEL_BASE|0xA0000); pos += 16) {
 133                 if( *(Uint*)(pos) == MPPTR_IDENT ) {
 134                         Log("Possible %p", pos);
 135                         if( ByteSum((void*)pos, sizeof(tMPInfo)) != 0 ) continue;
 136                         gMPFloatPtr = (void*)pos;
 137                         break;
 138                 }
 139         }
 140         // - Last KiB (512KiB base mem)
 141         if(!gMPFloatPtr) {
 142                 for(pos = KERNEL_BASE|0x7F000; pos < (KERNEL_BASE|0x80000); pos += 16) {
 143                         if( *(Uint*)(pos) == MPPTR_IDENT ) {
 144                                 Log("Possible %p", pos);
 145                                 if( ByteSum((void*)pos, sizeof(tMPInfo)) != 0 ) continue;
 146                                 gMPFloatPtr = (void*)pos;
 147                                 break;
 148                         }
 149                 }
 150         }
 151         // - BIOS ROM
 152         if(!gMPFloatPtr) {
 153                 for(pos = KERNEL_BASE|0xE0000; pos < (KERNEL_BASE|0x100000); pos += 16) {
 154                         if( *(Uint*)(pos) == MPPTR_IDENT ) {
 155                                 Log("Possible %p", pos);
 156                                 if( ByteSum((void*)pos, sizeof(tMPInfo)) != 0 ) continue;
 157                                 gMPFloatPtr = (void*)pos;
 158                                 break;
 159                         }
 160                 }
 161         }
 162
 163         // If the MP Table Exists, parse it
 164         if(gMPFloatPtr)
 165         {
 166                  int    i;
 167                 tMPTable_Ent    *ents;
 168                 Log("gMPFloatPtr = %p", gMPFloatPtr);
 169                 Log("*gMPFloatPtr = {");
 170                 Log("\t.Sig = 0x%08x", gMPFloatPtr->Sig);
 171                 Log("\t.MPConfig = 0x%08x", gMPFloatPtr->MPConfig);
 172                 Log("\t.Length = 0x%02x", gMPFloatPtr->Length);
 173                 Log("\t.Version = 0x%02x", gMPFloatPtr->Version);
 174                 Log("\t.Checksum = 0x%02x", gMPFloatPtr->Checksum);
 175                 Log("\t.Features = [0x%02x,0x%02x,0x%02x,0x%02x,0x%02x]",
 176                         gMPFloatPtr->Features[0],       gMPFloatPtr->Features[1],
 177                         gMPFloatPtr->Features[2],       gMPFloatPtr->Features[3],
 178                         gMPFloatPtr->Features[4]
 179                         );
 180                 Log("}");
 181
 182                 mptable = (void*)( KERNEL_BASE|gMPFloatPtr->MPConfig );
 183                 Log("mptable = %p", mptable);
 184                 Log("*mptable = {");
 185                 Log("\t.Sig = 0x%08x", mptable->Sig);
 186                 Log("\t.BaseTableLength = 0x%04x", mptable->BaseTableLength);
 187                 Log("\t.SpecRev = 0x%02x", mptable->SpecRev);
 188                 Log("\t.Checksum = 0x%02x", mptable->Checksum);
 189                 Log("\t.OEMID = '%8c'", mptable->OemID);
 190                 Log("\t.ProductID = '%8c'", mptable->ProductID);
 191                 Log("\t.OEMTablePtr = %p'", mptable->OEMTablePtr);
 192                 Log("\t.OEMTableSize = 0x%04x", mptable->OEMTableSize);
 193                 Log("\t.EntryCount = 0x%04x", mptable->EntryCount);
 194                 Log("\t.LocalAPICMemMap = 0x%08x", mptable->LocalAPICMemMap);
 195                 Log("\t.ExtendedTableLen = 0x%04x", mptable->ExtendedTableLen);
 196                 Log("\t.ExtendedTableChecksum = 0x%02x", mptable->ExtendedTableChecksum);
 197                 Log("}");
 198
 199                 gpMP_LocalAPIC = (void*)MM_MapHWPages(mptable->LocalAPICMemMap, 1);
 200
 201                 ents = mptable->Entries;
 202                 giNumCPUs = 0;
 203
 204                 for( i = 0; i < mptable->EntryCount; i ++ )
 205                 {
 206                          int    entSize = 0;
 207                         switch( ents->Type )
 208                         {
 209                         case 0: // Processor
 210                                 entSize = 20;
 211                                 Log("%i: Processor", i);
 212                                 Log("\t.APICID = %i", ents->Proc.APICID);
 213                                 Log("\t.APICVer = 0x%02x", ents->Proc.APICVer);
 214                                 Log("\t.CPUFlags = 0x%02x", ents->Proc.CPUFlags);
 215                                 Log("\t.CPUSignature = 0x%08x", ents->Proc.CPUSignature);
 216                                 Log("\t.FeatureFlags = 0x%08x", ents->Proc.FeatureFlags);
 217
 218
 219                                 if( !(ents->Proc.CPUFlags & 1) ) {
 220                                         Log("DISABLED");
 221                                         break;
 222                                 }
 223
 224                                 // Check if there is too many processors
 225                                 if(giNumCPUs >= MAX_CPUS) {
 226                                         giNumCPUs ++;   // If `giNumCPUs` > MAX_CPUS later, it will be clipped
 227                                         break;
 228                                 }
 229
 230                                 // Initialise CPU Info
 231                                 gaAPIC_to_CPU[ents->Proc.APICID] = giNumCPUs;
 232                                 gaCPUs[giNumCPUs].APICID = ents->Proc.APICID;
 233                                 gaCPUs[giNumCPUs].State = 0;
 234                                 giNumCPUs ++;
 235
 236                                 // Set BSP Variable
 237                                 if( ents->Proc.CPUFlags & 2 ) {
 238                                         giProc_BootProcessorID = giNumCPUs-1;
 239                                 }
 240
 241                                 break;
 242
 243                         #if DUMP_MP_TABLES
 244                         case 1: // Bus
 245                                 entSize = 8;
 246                                 Log("%i: Bus", i);
 247                                 Log("\t.ID = %i", ents->Bus.ID);
 248                                 Log("\t.TypeString = '%6C'", ents->Bus.TypeString);
 249                                 break;
 250                         case 2: // I/O APIC
 251                                 entSize = 8;
 252                                 Log("%i: I/O APIC", i);
 253                                 Log("\t.ID = %i", ents->IOAPIC.ID);
 254                                 Log("\t.Version = 0x%02x", ents->IOAPIC.Version);
 255                                 Log("\t.Flags = 0x%02x", ents->IOAPIC.Flags);
 256                                 Log("\t.Addr = 0x%08x", ents->IOAPIC.Addr);
 257                                 break;
 258                         case 3: // I/O Interrupt Assignment
 259                                 entSize = 8;
 260                                 Log("%i: I/O Interrupt Assignment", i);
 261                                 Log("\t.IntType = %i", ents->IOInt.IntType);
 262                                 Log("\t.Flags = 0x%04x", ents->IOInt.Flags);
 263                                 Log("\t.SourceBusID = 0x%02x", ents->IOInt.SourceBusID);
 264                                 Log("\t.SourceBusIRQ = 0x%02x", ents->IOInt.SourceBusIRQ);
 265                                 Log("\t.DestAPICID = 0x%02x", ents->IOInt.DestAPICID);
 266                                 Log("\t.DestAPICIRQ = 0x%02x", ents->IOInt.DestAPICIRQ);
 267                                 break;
 268                         case 4: // Local Interrupt Assignment
 269                                 entSize = 8;
 270                                 Log("%i: Local Interrupt Assignment", i);
 271                                 Log("\t.IntType = %i", ents->LocalInt.IntType);
 272                                 Log("\t.Flags = 0x%04x", ents->LocalInt.Flags);
 273                                 Log("\t.SourceBusID = 0x%02x", ents->LocalInt.SourceBusID);
 274                                 Log("\t.SourceBusIRQ = 0x%02x", ents->LocalInt.SourceBusIRQ);
 275                                 Log("\t.DestLocalAPICID = 0x%02x", ents->LocalInt.DestLocalAPICID);
 276                                 Log("\t.DestLocalAPICIRQ = 0x%02x", ents->LocalInt.DestLocalAPICIRQ);
 277                                 break;
 278                         default:
 279                                 Log("%i: Unknown (%i)", i, ents->Type);
 280                                 break;
 281                         #endif
 282                         }
 283                         ents = (void*)( (Uint)ents + entSize );
 284                 }
 285
 286                 if( giNumCPUs > MAX_CPUS ) {
 287                         Warning("Too many CPUs detected (%i), only using %i of them", giNumCPUs, MAX_CPUS);
 288                         giNumCPUs = MAX_CPUS;
 289                 }
 290                 gTSSs = gaTSSs;
 291         }
 292         else {
 293                 Log("No MP Table was found, assuming uniprocessor\n");
 294                 giNumCPUs = 1;
 295                 gTSSs = &gTSS0;
 296         }
 297         #else
 298         giNumCPUs = 1;
 299         gTSSs = &gTSS0;
 300         #endif
 301
 302         #if !DEBUG_DISABLE_DOUBLEFAULT
 303         // Initialise Double Fault TSS
 304         gGDT[5].BaseLow = (Uint)&gDoubleFault_TSS & 0xFFFF;
 305         gGDT[5].BaseMid = (Uint)&gDoubleFault_TSS >> 16;
 306         gGDT[5].BaseHi = (Uint)&gDoubleFault_TSS >> 24;
 307
 308         // Set double fault IDT to use the new TSS
 309         gIDT[8].OffsetLo = 0;
 310         gIDT[8].CS = 5<<3;
 311         gIDT[8].Flags = 0x8500;
 312         gIDT[8].OffsetHi = 0;
 313         #endif
 314
 315         // Set timer frequency
 316         outb(0x43, 0x34);       // Set Channel 0, Low/High, Rate Generator
 317         outb(0x40, TIMER_DIVISOR&0xFF); // Low Byte of Divisor
 318         outb(0x40, (TIMER_DIVISOR>>8)&0xFF);    // High Byte
 319
 320         Log_Debug("Proc", "PIT Frequency %i.%03i Hz",
 321                 TIMER_BASE/TIMER_DIVISOR,
 322                 ((Uint64)TIMER_BASE*1000/TIMER_DIVISOR)%1000
 323                 );
 324
 325         #if USE_MP
 326         // Get the count setting for APIC timer
 327         Log("Determining APIC Count");
 328         __asm__ __volatile__ ("sti");
 329         while( giMP_TimerCount == 0 )   __asm__ __volatile__ ("hlt");
 330         __asm__ __volatile__ ("cli");
 331         Log("APIC Count %i", giMP_TimerCount);
 332         {
 333                 Uint64  freq = giMP_TimerCount;
 334                 freq *= TIMER_BASE;
 335                 freq /= TIMER_DIVISOR;
 336                 if( (freq /= 1000) < 2*1000)
 337                         Log("Bus Frequency %i KHz", freq);
 338                 else if( (freq /= 1000) < 2*1000)
 339                         Log("Bus Frequency %i MHz", freq);
 340                 else if( (freq /= 1000) < 2*1000)
 341                         Log("Bus Frequency %i GHz", freq);
 342                 else
 343                         Log("Bus Frequency %i THz", freq);
 344         }
 345
 346         // Initialise Normal TSS(s)
 347         for(pos=0;pos<giNumCPUs;pos++)
 348         {
 349         #else
 350         pos = 0;
 351         #endif
 352                 gTSSs[pos].SS0 = 0x10;
 353                 gTSSs[pos].ESP0 = 0;    // Set properly by scheduler
 354                 gGDT[6+pos].BaseLow = ((Uint)(&gTSSs[pos])) & 0xFFFF;
 355                 gGDT[6+pos].BaseMid = ((Uint)(&gTSSs[pos]) >> 16) & 0xFFFF;
 356                 gGDT[6+pos].BaseHi = ((Uint)(&gTSSs[pos])) >> 24;
 357         #if USE_MP
 358         }
 359         #endif
 360
 361         // Load the BSP's TSS
 362         __asm__ __volatile__ ("ltr %%ax"::"a"(0x30));
 363         // Set Current Thread and CPU Number in DR0 and DR1
 364         __asm__ __volatile__ ("mov %0, %%db0"::"r"(&gThreadZero));
 365         __asm__ __volatile__ ("mov %0, %%db1"::"r"(0));
 366
 367         gaCPUs[0].Current = &gThreadZero;
 368         gThreadZero.CurCPU = 0;
 369
 370         gThreadZero.MemState.CR3 = (Uint)gaInitPageDir - KERNEL_BASE;
 371
 372         // Create Per-Process Data Block
 373         if( !MM_Allocate(MM_PPD_CFG) )
 374         {
 375                 Panic("OOM - No space for initial Per-Process Config");
 376         }
 377
 378         // Initialise SSE support
 379         Proc_InitialiseSSE();
 380
 381         // Change Stacks
 382         Proc_ChangeStack();
 383 }
 384
 385 #if USE_MP
 386 /**
 387  * \brief Start an AP
 388  */
 389 void MP_StartAP(int CPU)
 390 {
 391         Log_Log("Proc", "Starting AP %i (APIC %i)", CPU, gaCPUs[CPU].APICID);
 392
 393         // Set location of AP startup code and mark for a warm restart
 394         *(Uint16*)(KERNEL_BASE|0x467) = (Uint)&APWait - (KERNEL_BASE|0xFFFF0);
 395         *(Uint16*)(KERNEL_BASE|0x469) = 0xFFFF;
 396         outb(0x70, 0x0F);       outb(0x71, 0x0A);       // Set warm reset flag
 397         MP_SendIPI(gaCPUs[CPU].APICID, 0, 5);   // Init IPI
 398
 399         // Delay
 400         inb(0x80); inb(0x80); inb(0x80); inb(0x80);
 401
 402         // TODO: Use a better address, preferably registered with the MM
 403         // - MM_AllocDMA mabye?
 404         // Create a far jump
 405         *(Uint8*)(KERNEL_BASE|0x11000) = 0xEA;  // Far JMP
 406         *(Uint16*)(KERNEL_BASE|0x11001) = (Uint)&APStartup - (KERNEL_BASE|0xFFFF0);     // IP
 407         *(Uint16*)(KERNEL_BASE|0x11003) = 0xFFFF;       // CS
 408         // Send a Startup-IPI to make the CPU execute at 0x11000 (which we
 409         // just filled)
 410         MP_SendIPI(gaCPUs[CPU].APICID, 0x11, 6);        // StartupIPI
 411
 412         giNumInitingCPUs ++;
 413 }
 414
 415 void MP_SendIPIVector(int CPU, Uint8 Vector)
 416 {
 417         MP_SendIPI(gaCPUs[CPU].APICID, Vector, 0);
 418 }
 419
 420 /**
 421  * \brief Send an Inter-Processor Interrupt
 422  * \param APICID        Processor's Local APIC ID
 423  * \param Vector        Argument of some kind
 424  * \param DeliveryMode  Type of signal
 425  * \note 3A 10.5 "APIC/Handling Local Interrupts"
 426  */
 427 void MP_SendIPI(Uint8 APICID, int Vector, int DeliveryMode)
 428 {
 429         Uint32  val;
 430
 431         // Hi
 432         val = (Uint)APICID << 24;
 433 //      Log("%p = 0x%08x", &gpMP_LocalAPIC->ICR[1], val);
 434         gpMP_LocalAPIC->ICR[1].Val = val;
 435         // Low (and send)
 436         val = ((DeliveryMode & 7) << 8) | (Vector & 0xFF);
 437 //      Log("%p = 0x%08x", &gpMP_LocalAPIC->ICR[0], val);
 438         gpMP_LocalAPIC->ICR[0].Val = val;
 439 }
 440 #endif
 441
 442 void Proc_IdleThread(void *Ptr)
 443 {
 444         tCPU    *cpu = &gaCPUs[GetCPUNum()];
 445         cpu->Current->ThreadName = strdup("Idle Thread");
 446         Threads_SetPriority( cpu->Current, -1 );        // Never called randomly
 447         cpu->Current->Quantum = 1;      // 1 slice quantum
 448         for(;;) {
 449                 __asm__ __volatile__ ("sti");   // Make sure interrupts are enabled
 450                 __asm__ __volatile__ ("hlt");   // Make sure interrupts are enabled
 451                 Proc_Reschedule();
 452         }
 453 }
 454
 455 /**
 456  * \fn void Proc_Start(void)
 457  * \brief Start process scheduler
 458  */
 459 void Proc_Start(void)
 460 {
 461          int    tid;
 462         #if USE_MP
 463          int    i;
 464         #endif
 465
 466         #if USE_MP
 467         // Start APs
 468         for( i = 0; i < giNumCPUs; i ++ )
 469         {
 470                 if(i)   gaCPUs[i].Current = NULL;
 471
 472                 // Create Idle Task
 473                 tid = Proc_NewKThread(Proc_IdleThread, &gaCPUs[i]);
 474
 475                 // Start the AP
 476                 if( i != giProc_BootProcessorID ) {
 477                         MP_StartAP( i );
 478                 }
 479         }
 480
 481         // BSP still should run the current task
 482         gaCPUs[0].Current = &gThreadZero;
 483
 484         // Start interrupts and wait for APs to come up
 485         Log_Debug("Proc", "Waiting for APs to come up");
 486         __asm__ __volatile__ ("sti");
 487         while( giNumInitingCPUs )       __asm__ __volatile__ ("hlt");
 488         #else
 489         // Create Idle Task
 490         tid = Proc_NewKThread(Proc_IdleThread, &gaCPUs[0]);
 491 //      gaCPUs[0].IdleThread = Threads_GetThread(tid);
 492
 493         // Set current task
 494         gaCPUs[0].Current = &gThreadZero;
 495
 496         // Start Interrupts (and hence scheduler)
 497         __asm__ __volatile__("sti");
 498         #endif
 499         MM_FinishVirtualInit();
 500 }
 501
 502 /**
 503  * \fn tThread *Proc_GetCurThread(void)
 504  * \brief Gets the current thread
 505  */
 506 tThread *Proc_GetCurThread(void)
 507 {
 508         #if USE_MP
 509         return gaCPUs[ GetCPUNum() ].Current;
 510         #else
 511         return gaCPUs[ 0 ].Current;
 512         #endif
 513 }
 514
 515 /**
 516  * \fn void Proc_ChangeStack(void)
 517  * \brief Swaps the current stack for a new one (in the proper stack reigon)
 518  */
 519 void Proc_ChangeStack(void)
 520 {
 521         Uint    esp, ebp;
 522         Uint    tmpEbp, oldEsp;
 523         Uint    curBase, newBase;
 524
 525         __asm__ __volatile__ ("mov %%esp, %0":"=r"(esp));
 526         __asm__ __volatile__ ("mov %%ebp, %0":"=r"(ebp));
 527
 528         oldEsp = esp;
 529
 530         // Create new KStack
 531         newBase = MM_NewKStack();
 532         // Check for errors
 533         if(newBase == 0) {
 534                 Panic("What the?? Unable to allocate space for initial kernel stack");
 535                 return;
 536         }
 537
 538         curBase = (Uint)&Kernel_Stack_Top;
 539
 540         LOG("curBase = 0x%x, newBase = 0x%x", curBase, newBase);
 541
 542         // Get ESP as a used size
 543         esp = curBase - esp;
 544         LOG("memcpy( %p, %p, 0x%x )", (void*)(newBase - esp), (void*)(curBase - esp), esp );
 545         // Copy used stack
 546         memcpy( (void*)(newBase - esp), (void*)(curBase - esp), esp );
 547         // Get ESP as an offset in the new stack
 548         esp = newBase - esp;
 549         // Adjust EBP
 550         ebp = newBase - (curBase - ebp);
 551
 552         // Repair EBPs & Stack Addresses
 553         // Catches arguments also, but may trash stack-address-like values
 554         for(tmpEbp = esp; tmpEbp < newBase; tmpEbp += 4)
 555         {
 556                 if(oldEsp < *(Uint*)tmpEbp && *(Uint*)tmpEbp < curBase)
 557                         *(Uint*)tmpEbp += newBase - curBase;
 558         }
 559
 560         Proc_GetCurThread()->KernelStack = newBase;
 561
 562         __asm__ __volatile__ ("mov %0, %%esp"::"r"(esp));
 563         __asm__ __volatile__ ("mov %0, %%ebp"::"r"(ebp));
 564 }
 565
 566 void Proc_ClearThread(tThread *Thread)
 567 {
 568         Log_Warning("Proc", "TODO: Nuke address space etc");
 569         if(Thread->SavedState.SSE) {
 570                 free(Thread->SavedState.SSE);
 571                 Thread->SavedState.SSE = NULL;
 572         }
 573 }
 574
 575 int Proc_NewKThread(void (*Fcn)(void*), void *Data)
 576 {
 577         Uint    esp;
 578         tThread *newThread, *cur;
 579
 580         cur = Proc_GetCurThread();
 581         newThread = Threads_CloneTCB(0);
 582         if(!newThread)  return -1;
 583
 584         // Set CR3
 585         newThread->MemState.CR3 = cur->MemState.CR3;
 586
 587         // Create new KStack
 588         newThread->KernelStack = MM_NewKStack();
 589         // Check for errors
 590         if(newThread->KernelStack == 0) {
 591                 free(newThread);
 592                 return -1;
 593         }
 594
 595         esp = newThread->KernelStack;
 596         *(Uint*)(esp-=4) = (Uint)Data;  // Data (shadowed)
 597         *(Uint*)(esp-=4) = 1;   // Number of params
 598         *(Uint*)(esp-=4) = (Uint)Fcn;   // Function to call
 599         *(Uint*)(esp-=4) = (Uint)newThread;     // Thread ID
 600
 601         newThread->SavedState.ESP = esp;
 602         newThread->SavedState.EIP = (Uint)&NewTaskHeader;
 603         newThread->SavedState.SSE = NULL;
 604 //      Log("New (KThread) %p, esp = %p", newThread->SavedState.EIP, newThread->SavedState.ESP);
 605
 606 //      MAGIC_BREAK();
 607         Threads_AddActive(newThread);
 608
 609         return newThread->TID;
 610 }
 611
 612 /**
 613  * \fn int Proc_Clone(Uint *Err, Uint Flags)
 614  * \brief Clone the current process
 615  */
 616 int Proc_Clone(Uint Flags)
 617 {
 618         tThread *newThread;
 619         tThread *cur = Proc_GetCurThread();
 620         Uint    eip;
 621
 622         // Sanity, please
 623         if( !(Flags & CLONE_VM) ) {
 624                 Log_Error("Proc", "Proc_Clone: Don't leave CLONE_VM unset, use Proc_NewKThread instead");
 625                 return -1;
 626         }
 627
 628         // New thread
 629         newThread = Threads_CloneTCB(Flags);
 630         if(!newThread)  return -1;
 631
 632         newThread->KernelStack = cur->KernelStack;
 633
 634         // Clone state
 635         eip = Proc_CloneInt(&newThread->SavedState.ESP, &newThread->MemState.CR3);
 636         if( eip == 0 ) {
 637                 // ACK the interrupt
 638                 return 0;
 639         }
 640         newThread->SavedState.EIP = eip;
 641         newThread->SavedState.SSE = NULL;
 642         newThread->SavedState.bSSEModified = 0;
 643
 644         // Check for errors
 645         if( newThread->MemState.CR3 == 0 ) {
 646                 Log_Error("Proc", "Proc_Clone: MM_Clone failed");
 647                 Threads_Delete(newThread);
 648                 return -1;
 649         }
 650
 651         // Add the new thread to the run queue
 652         Threads_AddActive(newThread);
 653         return newThread->TID;
 654 }
 655
 656 /**
 657  * \fn int Proc_SpawnWorker(void)
 658  * \brief Spawns a new worker thread
 659  */
 660 int Proc_SpawnWorker(void (*Fcn)(void*), void *Data)
 661 {
 662         tThread *new;
 663         Uint    stack_contents[4];
 664
 665         // Create new thread
 666         new = Threads_CloneThreadZero();
 667         if(!new) {
 668                 Warning("Proc_SpawnWorker - Out of heap space!\n");
 669                 return -1;
 670         }
 671
 672         // Create the stack contents
 673         stack_contents[3] = (Uint)Data;
 674         stack_contents[2] = 1;
 675         stack_contents[1] = (Uint)Fcn;
 676         stack_contents[0] = (Uint)new;
 677
 678         // Create a new worker stack (in PID0's address space)
 679         new->KernelStack = MM_NewWorkerStack(stack_contents, sizeof(stack_contents));
 680
 681         // Save core machine state
 682         new->SavedState.ESP = new->KernelStack - sizeof(stack_contents);
 683         new->SavedState.EIP = (Uint)NewTaskHeader;
 684         new->SavedState.SSE = NULL;
 685         new->SavedState.bSSEModified = 0;
 686
 687         // Mark as active
 688         new->Status = THREAD_STAT_PREINIT;
 689         Threads_AddActive( new );
 690
 691         return new->TID;
 692 }
 693
 694 /**
 695  * \fn Uint Proc_MakeUserStack(void)
 696  * \brief Creates a new user stack
 697  */
 698 Uint Proc_MakeUserStack(void)
 699 {
 700          int    i;
 701         Uint    base = USER_STACK_TOP - USER_STACK_SZ;
 702
 703         // Check Prospective Space
 704         for( i = USER_STACK_SZ >> 12; i--; )
 705                 if( MM_GetPhysAddr( base + (i<<12) ) != 0 )
 706                         break;
 707
 708         if(i != -1)     return 0;
 709
 710         // Allocate Stack - Allocate incrementally to clean up MM_Dump output
 711         for( i = 0; i < USER_STACK_SZ/0x1000; i++ )
 712         {
 713                 if( !MM_Allocate( base + (i<<12) ) )
 714                 {
 715                         Warning("OOM: Proc_MakeUserStack");
 716                         return 0;
 717                 }
 718         }
 719
 720         return base + USER_STACK_SZ;
 721 }
 722
 723 void Proc_StartUser(Uint Entrypoint, Uint Base, int ArgC, char **ArgV, int DataSize)
 724 {
 725         Uint    *stack;
 726          int    i;
 727         char    **envp = NULL;
 728         Uint16  ss, cs;
 729
 730         // Copy data to the user stack and free original buffer
 731         stack = (void*)Proc_MakeUserStack();
 732         stack -= DataSize/sizeof(*stack);
 733         memcpy( stack, ArgV, DataSize );
 734         free(ArgV);
 735
 736         // Adjust Arguments and environment
 737         if( DataSize )
 738         {
 739                 Uint delta = (Uint)stack - (Uint)ArgV;
 740                 ArgV = (char**)stack;
 741                 for( i = 0; ArgV[i]; i++ )      ArgV[i] += delta;
 742                 envp = &ArgV[i+1];
 743                 for( i = 0; envp[i]; i++ )      envp[i] += delta;
 744         }
 745
 746         // User Mode Segments
 747         ss = 0x23;      cs = 0x1B;
 748
 749         // Arguments
 750         *--stack = (Uint)envp;
 751         *--stack = (Uint)ArgV;
 752         *--stack = (Uint)ArgC;
 753         *--stack = Base;
 754
 755         Proc_StartProcess(ss, (Uint)stack, 0x202, cs, Entrypoint);
 756 }
 757
 758 void Proc_StartProcess(Uint16 SS, Uint Stack, Uint Flags, Uint16 CS, Uint IP)
 759 {
 760         Uint    *stack = (void*)Stack;
 761         *--stack = SS;          //Stack Segment
 762         *--stack = Stack;       //Stack Pointer
 763         *--stack = Flags;       //EFLAGS (Resvd (0x2) and IF (0x20))
 764         *--stack = CS;          //Code Segment
 765         *--stack = IP;  //EIP
 766         //PUSHAD
 767         *--stack = 0xAAAAAAAA;  // eax
 768         *--stack = 0xCCCCCCCC;  // ecx
 769         *--stack = 0xDDDDDDDD;  // edx
 770         *--stack = 0xBBBBBBBB;  // ebx
 771         *--stack = 0xD1D1D1D1;  // edi
 772         *--stack = 0x54545454;  // esp - NOT POPED
 773         *--stack = 0x51515151;  // esi
 774         *--stack = 0xB4B4B4B4;  // ebp
 775         //Individual PUSHs
 776         *--stack = SS;  // ds
 777         *--stack = SS;  // es
 778         *--stack = SS;  // fs
 779         *--stack = SS;  // gs
 780
 781         __asm__ __volatile__ (
 782         "mov %%eax,%%esp;\n\t"  // Set stack pointer
 783         "pop %%gs;\n\t"
 784         "pop %%fs;\n\t"
 785         "pop %%es;\n\t"
 786         "pop %%ds;\n\t"
 787         "popa;\n\t"
 788         "iret;\n\t" : : "a" (stack));
 789         for(;;);
 790 }
 791
 792 /**
 793  * \fn int Proc_Demote(Uint *Err, int Dest, tRegs *Regs)
 794  * \brief Demotes a process to a lower permission level
 795  * \param Err   Pointer to user's errno
 796  * \param Dest  New Permission Level
 797  * \param Regs  Pointer to user's register structure
 798  */
 799 int Proc_Demote(Uint *Err, int Dest, tRegs *Regs)
 800 {
 801          int    cpl = Regs->cs & 3;
 802         // Sanity Check
 803         if(Dest > 3 || Dest < 0) {
 804                 *Err = -EINVAL;
 805                 return -1;
 806         }
 807
 808         // Permission Check
 809         if(cpl > Dest) {
 810                 *Err = -EACCES;
 811                 return -1;
 812         }
 813
 814         // Change the Segment Registers
 815         Regs->cs = (((Dest+1)<<4) | Dest) - 8;
 816         Regs->ss = ((Dest+1)<<4) | Dest;
 817         // Check if the GP Segs are GDT, then change them
 818         if(!(Regs->ds & 4))     Regs->ds = ((Dest+1)<<4) | Dest;
 819         if(!(Regs->es & 4))     Regs->es = ((Dest+1)<<4) | Dest;
 820         if(!(Regs->fs & 4))     Regs->fs = ((Dest+1)<<4) | Dest;
 821         if(!(Regs->gs & 4))     Regs->gs = ((Dest+1)<<4) | Dest;
 822
 823         return 0;
 824 }
 825
 826 /**
 827  * \brief Calls a signal handler in user mode
 828  * \note Used for signals
 829  */
 830 void Proc_CallFaultHandler(tThread *Thread)
 831 {
 832         // Rewinds the stack and calls the user function
 833         // Never returns
 834         Proc_ReturnToUser( Thread->FaultHandler, Thread->CurFaultNum, Thread->KernelStack );
 835         for(;;);
 836 }
 837
 838 void Proc_DumpThreadCPUState(tThread *Thread)
 839 {
 840         if( Thread->CurCPU > -1 )
 841         {
 842                  int    maxBacktraceDistance = 6;
 843                 tRegs   *regs = NULL;
 844                 Uint32  *stack;
 845
 846                 if( Thread->CurCPU != GetCPUNum() ) {
 847                         Log("  Currently running");
 848                         return ;
 849                 }
 850
 851                 // Backtrace to find the IRQ entrypoint
 852                 // - This will usually only be called by an IRQ, so this should
 853                 //   work
 854                 __asm__ __volatile__ ("mov %%ebp, %0" : "=r" (stack));
 855                 while( maxBacktraceDistance -- )
 856                 {
 857                         // [ebp] = oldEbp
 858                         // [ebp+4] = retaddr
 859
 860                         if( stack[1] == (tVAddr)&IRQCommon_handled ) {
 861                                 regs = (void*)stack[2];
 862                                 break;
 863                         }
 864
 865                         stack = (void*)stack[0];
 866                 }
 867
 868                 if( !regs ) {
 869                         Log("  Unable to find IRQ Entry");
 870                         return ;
 871                 }
 872
 873                 Log("  at %04x:%08x", regs->cs, regs->eip);
 874                 return ;
 875         }
 876
 877         tVAddr  diffFromScheduler = Thread->SavedState.EIP - (tVAddr)SwitchTasks;
 878         tVAddr  diffFromClone = Thread->SavedState.EIP - (tVAddr)Proc_CloneInt;
 879         tVAddr  diffFromSpawn = Thread->SavedState.EIP - (tVAddr)NewTaskHeader;
 880
 881         if( diffFromClone > 0 && diffFromClone < 40 )   // When I last checked, .newTask was at .+27
 882         {
 883                 Log("  Creating process");
 884                 return ;
 885         }
 886
 887         if( diffFromSpawn == 0 )
 888         {
 889                 Log("  Creating thread");
 890                 return ;
 891         }
 892
 893         if( diffFromScheduler > 0 && diffFromScheduler < 128 )  // When I last checked, GetEIP was at .+0x30
 894         {
 895                 // Scheduled out
 896                 Log("  At %04x:%08x", Thread->SavedState.UserCS, Thread->SavedState.UserEIP);
 897                 return ;
 898         }
 899
 900         Log("  Just created (unknown %p)", Thread->SavedState.EIP);
 901 }
 902
 903 void Proc_Reschedule(void)
 904 {
 905         tThread *nextthread, *curthread;
 906          int    cpu = GetCPUNum();
 907
 908         // TODO: Wait for the lock?
 909         if(IS_LOCKED(&glThreadListLock))        return;
 910
 911         curthread = Proc_GetCurThread();
 912
 913         nextthread = Threads_GetNextToRun(cpu, curthread);
 914
 915         if(!nextthread || nextthread == curthread)
 916                 return ;
 917
 918         #if DEBUG_TRACE_SWITCH
 919         // HACK: Ignores switches to the idle threads
 920         if( nextthread->TID == 0 || nextthread->TID > giNumCPUs )
 921         {
 922                 LogF("\nSwitching CPU %i to %p (%i %s) - CR3 = 0x%x, EIP = %p, ESP = %p\n",
 923                         GetCPUNum(),
 924                         nextthread, nextthread->TID, nextthread->ThreadName,
 925                         nextthread->MemState.CR3,
 926                         nextthread->SavedState.EIP,
 927                         nextthread->SavedState.ESP
 928                         );
 929         }
 930         #endif
 931
 932         // Update CPU state
 933         gaCPUs[cpu].Current = nextthread;
 934         gTSSs[cpu].ESP0 = nextthread->KernelStack-4;
 935         __asm__ __volatile__("mov %0, %%db0\n\t" : : "r"(nextthread) );
 936
 937         // Save FPU/MMX/XMM/SSE state
 938         if( curthread && curthread->SavedState.SSE )
 939         {
 940                 Proc_SaveSSE( ((Uint)curthread->SavedState.SSE + 0xF) & ~0xF );
 941                 curthread->SavedState.bSSEModified = 0;
 942                 Proc_DisableSSE();
 943         }
 944
 945         SwitchTasks(
 946                 nextthread->SavedState.ESP, &curthread->SavedState.ESP,
 947                 nextthread->SavedState.EIP, &curthread->SavedState.EIP,
 948                 nextthread->MemState.CR3
 949                 );
 950
 951         return ;
 952 }
 953
 954 /**
 955  * \fn void Proc_Scheduler(int CPU)
 956  * \brief Swap current thread and clears dead threads
 957  */
 958 void Proc_Scheduler(int CPU)
 959 {
 960         tThread *thread;
 961
 962         // If the spinlock is set, let it complete
 963         if(IS_LOCKED(&glThreadListLock))        return;
 964
 965         // Get current thread
 966         thread = gaCPUs[CPU].Current;
 967
 968         if( thread )
 969         {
 970                 tRegs   *regs;
 971                 Uint    ebp;
 972                 // Reduce remaining quantum and continue timeslice if non-zero
 973                 if( thread->Remaining-- )
 974                         return;
 975                 // Reset quantum for next call
 976                 thread->Remaining = thread->Quantum;
 977
 978                 // TODO: Make this more stable somehow
 979                 __asm__ __volatile__("mov %%ebp, %0" : "=r" (ebp));
 980                 regs = (tRegs*)(ebp+(2+2)*4);   // EBP,Ret + CPU,CurThread
 981                 thread->SavedState.UserCS = regs->cs;
 982                 thread->SavedState.UserEIP = regs->eip;
 983
 984                 if(thread->bInstrTrace) {
 985                         regs->eflags |= 0x100;  // Set TF
 986                         Log("%p De-scheduled", thread);
 987                 }
 988                 else
 989                         regs->eflags &= ~0x100; // Clear TF
 990         }
 991
 992 #if 0
 993         // TODO: Ack timer?
 994         #if USE_MP
 995         if( GetCPUNum() )
 996                 gpMP_LocalAPIC->EOI.Val = 0;
 997         else
 998         #endif
 999                 outb(0x20, 0x20);
1000         __asm__ __volatile__ ("sti");
1001         Proc_Reschedule();
1002 #endif
1003 }
1004
1005 // === EXPORTS ===
1006 EXPORT(Proc_SpawnWorker);