+/**
+ * \fn Uint MM_NewWorkerStack()
+ * \brief Creates a new worker stack
+ */
+Uint MM_NewWorkerStack()
+{
+ Uint esp, ebp;
+ Uint oldstack;
+ Uint base, addr;
+ int i, j;
+ Uint *tmpPage;
+ tPAddr pages[WORKER_STACK_SIZE>>12];
+
+ // Get the old ESP and EBP
+ __asm__ __volatile__ ("mov %%esp, %0": "=r"(esp));
+ __asm__ __volatile__ ("mov %%ebp, %0": "=r"(ebp));
+
+ // Find a free worker stack address
+ for(base = giLastUsedWorker; base < NUM_WORKER_STACKS; base++)
+ {
+ // Used block
+ if( gWorkerStacks[base/32] == -1 ) {
+ base += 31; base &= ~31;
+ base --; // Counteracted by the base++
+ continue;
+ }
+ // Used stack
+ if( gWorkerStacks[base/32] & (1 << base) ) {
+ continue;
+ }
+ }
+ if(base >= NUM_WORKER_STACKS) {
+ Warning("Uh-oh! Out of worker stacks");
+ return 0;
+ }
+
+ // It's ours now!
+ gWorkerStacks[base/32] |= (1 << base);
+ // Make life easier for later calls
+ giLastUsedWorker = base;
+ // We have one
+ base = WORKER_STACKS + base * WORKER_STACK_SIZE;
+
+ // Acquire the lock for the temp fractal mappings
+ LOCK(&gilTempFractal);
+
+ // Set the temp fractals to TID0's address space
+ *gTmpCR3 = (Uint)gaInitPageDir | 3;
+ INVLPG( gaTmpDir );
+
+ // Check if the directory is mapped (we are assuming that the stacks
+ // will fit neatly in a directory
+ if(gaTmpDir[ base >> 22 ] == 0) {
+ gaTmpDir[ base >> 22 ] = MM_AllocPhys() | 3;
+ INVLPG( &gaTmpTable[ (base>>22) & ~0x3FF ] );
+ }
+
+ // Mapping Time!
+ for( addr = 0; addr < WORKER_STACK_SIZE; addr += 0x1000 )
+ {
+ pages[ addr >> 12 ] = MM_AllocPhys();
+ gaTmpTable[ (base + addr) >> 12 ] = pages[addr>>12] | 3;
+ }
+ // Release the temp mapping lock
+ RELEASE(&gilTempFractal);
+
+ // Copy the old stack
+ oldstack = (esp + KERNEL_STACK_SIZE-1) & ~(KERNEL_STACK_SIZE-1);
+ esp = oldstack - esp; // ESP as an offset in the stack
+
+ i = (WORKER_STACK_SIZE>>12) - 1;
+ // Copy the contents of the old stack to the new one, altering the addresses
+ // `addr` is refering to bytes from the stack base (mem downwards)
+ for(addr = 0; addr < esp; addr += 0x1000)
+ {
+ Uint *stack = (Uint*)( oldstack-(addr+0x1000) );
+ tmpPage = (void*)MM_MapTemp( pages[i] );
+ // Copy old stack
+ for(j = 0; j < 1024; j++)
+ {
+ // Possible Stack address?
+ if(oldstack-esp < stack[j] && stack[j] < oldstack)
+ tmpPage[j] = base - (oldstack - stack[j]);
+ else // Seems not, best leave it alone
+ tmpPage[j] = stack[j];
+ }
+ MM_FreeTemp((Uint)tmpPage);
+ i --;
+ }
+
+ return base;
+}
+