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