- Added another MSVC intrinsic to gcc (BitScanReverse). Thanks to Vampyre.

- Added very basic and skeletal NUMA code when creating a thread and process, currently only does some basic affinity checks and settings.
- Added a table and helper function (KeFindNextRightSetAffinity) for calculating affinity masks and sets.
- Split KeInitailizeThread into KeStartThread and KeInitThread, and modified Ps code to use the calls. Now added a failure case where Init fails, but we don't have to backout the entire effects of a "Start".
- Changes based on WI4 and Windows Internals II.

svn path=/trunk/; revision=23111

reactos/include/psdk/winnt.h

@@ -3814,6 +3814,19 @@ InterlockedBitTestAndReset(IN LONG volatile *Base,
 	return OldBit;
 }
 
+static __inline__ BOOLEAN
+BitScanReverse(OUT ULONG *Index,
+               IN ULONG Mask)
+{
+	LONG OldBit;
+	__asm__ __volatile__("bsrl %1,%2\n\t"
+	                     "sbbl %0,%0\n\t"
+		             :"=r" (OldBit),"=m" (*Index)
+		             :"Ir" (Mask)
+			     : "memory");
+	return OldBit;
+}
+
 #endif
 
 #define YieldProcessor() __asm__ __volatile__("pause");

reactos/ntoskrnl/include/internal/ke.h

@@ -44,6 +44,9 @@ extern ULONG_PTR KERNEL_BASE;
 extern ULONG KeI386NpxPresent;
 extern ULONG KeI386XMMIPresent;
 extern ULONG KeI386FxsrPresent;
+extern PKNODE KeNodeBlock[1];
+extern UCHAR KeNumberNodes;
+extern UCHAR KeProcessNodeSeed;
 
 /* MACROS *************************************************************************/
 
@@ -68,6 +71,8 @@ extern ULONG KeI386FxsrPresent;
 #define KeReleaseDispatcherDatabaseLockFromDpcLevel()
 #endif
 
+#define AFFINITY_MASK(Id) KiMask32Array[Id]
+
 /* The following macro initializes a dispatcher object's header */
 #define KeInitializeDispatcherHeader(Header, t, s, State)                   \
 {                                                                           \
@@ -188,6 +193,13 @@ KiIpiSendRequest(
 
 /* next file ***************************************************************/
 
+UCHAR
+NTAPI
+KeFindNextRightSetAffinity(
+    IN UCHAR Number,
+    IN ULONG Set
+);
+
 VOID 
 STDCALL
 DbgBreakPointNoBugCheck(VOID);
@@ -267,16 +279,35 @@ KiExpireTimers(
 );
 
 VOID
-STDCALL
+NTAPI
 KeInitializeThread(
-    struct _KPROCESS* Process,
+    IN PKPROCESS Process,
-    PKTHREAD Thread,
+    IN OUT PKTHREAD Thread,
-    PKSYSTEM_ROUTINE SystemRoutine,
+    IN PKSYSTEM_ROUTINE SystemRoutine,
-    PKSTART_ROUTINE StartRoutine,
+    IN PKSTART_ROUTINE StartRoutine,
-    PVOID StartContext,
+    IN PVOID StartContext,
-    PCONTEXT Context,
+    IN PCONTEXT Context,
-    PVOID Teb,
+    IN PVOID Teb,
-    PVOID KernelStack
+    IN PVOID KernelStack
+);
+
+NTSTATUS
+NTAPI
+KeInitThread(
+    IN OUT PKTHREAD Thread,
+    IN PVOID KernelStack,
+    IN PKSYSTEM_ROUTINE SystemRoutine,
+    IN PKSTART_ROUTINE StartRoutine,
+    IN PVOID StartContext,
+    IN PCONTEXT Context,
+    IN PVOID Teb,
+    IN PKPROCESS Process
+);
+
+VOID
+NTAPI
+KeStartThread(
+    IN OUT PKTHREAD Thread
 );
 
 VOID

reactos/ntoskrnl/ke/i386/kernel.c

@@ -16,6 +16,10 @@
 
 /* GLOBALS *******************************************************************/
 
+KNODE KiNode0;
+PKNODE KeNodeBlock[1];
+UCHAR KeNumberNodes = 1;
+UCHAR KeProcessNodeSeed;
 ULONG KiPcrInitDone = 0;
 static ULONG PcrsAllocated = 0;
 static ULONG Ke386CpuidFlags2, Ke386CpuidExFlags, Ke386CpuidExMisc;
@@ -340,6 +344,10 @@ KeInit1(PCHAR CommandLine, PULONG LastKernelAddress)
 
    KeActiveProcessors |= 1 << 0;
 
+    /* Set Node Data */
+    KeNodeBlock[0] = &KiNode0;
+    KPCR->PrcbData.ParentNode = KeNodeBlock[0];
+    KeNodeBlock[0]->ProcessorMask = KPCR->PrcbData.SetMember;
 
    if (KPCR->PrcbData.FeatureBits & X86_FEATURE_PGE)
    {

reactos/ntoskrnl/ke/kthread.c

@@ -24,8 +24,37 @@ static ULONG PriorityListMask = 0;
 ULONG IdleProcessorMask = 0;
 extern LIST_ENTRY PspReaperListHead;
 
+ULONG KiMask32Array[MAXIMUM_PRIORITY] =
+{
+    0x1,        0x2,       0x4,       0x8,       0x10,       0x20,
+    0x40,       0x80,      0x100,     0x200,     0x4000,     0x800,
+    0x1000,     0x2000,    0x4000,    0x8000,    0x10000,    0x20000,
+    0x40000,    0x80000,   0x100000,  0x200000,  0x400000,   0x800000,
+    0x1000000,  0x2000000, 0x4000000, 0x8000000, 0x10000000, 0x20000000,
+    0x40000000, 0x80000000
+};
+
 /* FUNCTIONS *****************************************************************/
 
+UCHAR
+NTAPI
+KeFindNextRightSetAffinity(IN UCHAR Number,
+                           IN ULONG Set)
+{
+    ULONG Bit, Result;
+    ASSERT(Set != 0);
+
+    /* Calculate the mask */
+    Bit = (AFFINITY_MASK(Number) - 1) & Set;
+
+    /* If it's 0, use the one we got */
+    if (!Bit) Bit = Set;
+
+    /* Now find the right set and return it */
+    BitScanReverse(&Result, Bit);
+    return (UCHAR)Result;
+}
+
 STATIC
 VOID
 KiRequestReschedule(CCHAR Processor)
@@ -699,35 +728,29 @@ KeCapturePersistentThreadState(IN PVOID CurrentThread,
     UNIMPLEMENTED;
 }
 
-/*
+NTSTATUS
- * FUNCTION: Initialize the microkernel state of the thread
+NTAPI
- */
+KeInitThread(IN OUT PKTHREAD Thread,
-VOID
+             IN PVOID KernelStack,
-STDCALL
+             IN PKSYSTEM_ROUTINE SystemRoutine,
-KeInitializeThread(PKPROCESS Process,
+             IN PKSTART_ROUTINE StartRoutine,
-                   PKTHREAD Thread,
+             IN PVOID StartContext,
-                   PKSYSTEM_ROUTINE SystemRoutine,
+             IN PCONTEXT Context,
-                   PKSTART_ROUTINE StartRoutine,
+             IN PVOID Teb,
-                   PVOID StartContext,
+             IN PKPROCESS Process)
-                   PCONTEXT Context,
-                   PVOID Teb,
-                   PVOID KernelStack)
 {
+    BOOLEAN AllocatedStack = FALSE;
     ULONG i;
     PKWAIT_BLOCK TimerWaitBlock;
     PKTIMER Timer;
+    NTSTATUS Status;
 
     /* Initalize the Dispatcher Header */
-    DPRINT("Initializing Dispatcher Header for New Thread: %x in Process: %x\n", Thread, Process);
     KeInitializeDispatcherHeader(&Thread->DispatcherHeader,
                                  ThreadObject,
                                  sizeof(KTHREAD) / sizeof(LONG),
                                  FALSE);
 
-    DPRINT("Thread Header Created. SystemRoutine: %x, StartRoutine: %x with Context: %x\n",
-            SystemRoutine, StartRoutine, StartContext);
-    DPRINT("UserMode Information. Context: %x, Teb: %x\n", Context, Teb);
-
     /* Initialize the Mutant List */
     InitializeListHead(&Thread->MutantListHead);
 
@@ -738,6 +761,15 @@ KeInitializeThread(PKPROCESS Process,
         Thread->WaitBlock[i].Thread = Thread;
     }
 
+    /* Set swap settings */
+    Thread->EnableStackSwap = FALSE;//TRUE;
+    Thread->IdealProcessor = 1;
+    Thread->SwapBusy = FALSE;
+    Thread->AdjustReason = 0;
+
+    /* Initialize the lock */
+    KeInitializeSpinLock(&Thread->ThreadLock);
+
     /* Setup the Service Descriptor Table for Native Calls */
     Thread->ServiceTable = KeServiceDescriptorTable;
 
@@ -762,7 +794,7 @@ KeInitializeThread(PKPROCESS Process,
                     NULL);
 
     /* Initialize the Suspend Semaphore */
-    KeInitializeSemaphore(&Thread->SuspendSemaphore, 0, 128);
+    KeInitializeSemaphore(&Thread->SuspendSemaphore, 0, 2);
 
     /* Setup the timer */
     Timer = &Thread->Timer;
@@ -780,53 +812,148 @@ KeInitializeThread(PKPROCESS Process,
     /* Set the TEB */
     Thread->Teb = Teb;
 
+    /* Check if we have a kernel stack */
+    if (!KernelStack)
+    {
+        /* We don't, allocate one */
+        KernelStack = (PVOID)((ULONG_PTR)MmCreateKernelStack(FALSE) +
+                              KERNEL_STACK_SIZE);
+        if (!KernelStack) return STATUS_INSUFFICIENT_RESOURCES;
+
+        /* Remember for later */
+        AllocatedStack = TRUE;
+    }
+
     /* Set the Thread Stacks */
     Thread->InitialStack = (PCHAR)KernelStack;
     Thread->StackBase = (PCHAR)KernelStack;
     Thread->StackLimit = (ULONG_PTR)KernelStack - KERNEL_STACK_SIZE;
     Thread->KernelStackResident = TRUE;
 
-    /*
+    /* ROS Mm HACK */
-     * Establish the pde's for the new stack and the thread structure within the
+    MmUpdatePageDir((PEPROCESS)Process,
-     * address space of the new process. They are accessed while taskswitching or
+                    (PVOID)Thread->StackLimit,
-     * while handling page faults. At this point it isn't possible to call the
+                    KERNEL_STACK_SIZE);
-     * page fault handler for the missing pde's.
-     */
-    MmUpdatePageDir((PEPROCESS)Process, (PVOID)Thread->StackLimit, KERNEL_STACK_SIZE);
     MmUpdatePageDir((PEPROCESS)Process, (PVOID)Thread, sizeof(ETHREAD));
 
-    /* Initalize the Thread Context */
+    /* Enter SEH to avoid crashes due to user mode */
-    DPRINT("Initializing the Context for the thread: %x\n", Thread);
+    Status = STATUS_SUCCESS;
-    KiArchInitThreadWithContext(Thread,
+    _SEH_TRY
-                                SystemRoutine,
+    {
-                                StartRoutine,
+        /* Initalize the Thread Context */
-                                StartContext,
+        KiArchInitThreadWithContext(Thread,
-                                Context);
+                                    SystemRoutine,
+                                    StartRoutine,
+                                    StartContext,
+                                    Context);
+    }
+    _SEH_HANDLE
+    {
+        /* Set failure status */
+        Status = STATUS_UNSUCCESSFUL;
 
-    /* Setup scheduler Fields based on Parent */
+        /* Check if a stack was allocated */
-    DPRINT("Thread context created, setting Scheduler Data\n");
+        if (AllocatedStack)
-    Thread->BasePriority = Process->BasePriority;
+        {
-    Thread->Quantum = Process->QuantumReset;
+            /* Delete the stack */
-    Thread->QuantumReset = Process->QuantumReset;
+            MmDeleteKernelStack(Thread->StackBase, FALSE);
-    Thread->Affinity = Process->Affinity;
+            Thread->InitialStack = NULL;
-    Thread->Priority = Process->BasePriority;
+        }
-    Thread->UserAffinity = Process->Affinity;
+    }
-    Thread->DisableBoost = Process->DisableBoost;
+    _SEH_END;
-    Thread->AutoAlignment = Process->AutoAlignment;
-    Thread->Iopl = Process->Iopl;
 
     /* Set the Thread to initalized */
     Thread->State = Initialized;
-
+    return Status;
-    /*
-     * Insert the Thread into the Process's Thread List
-     * Note, this is the KTHREAD Thread List. It is removed in
-     * ke/kthread.c!KeTerminateThread.
-     */
-    InsertTailList(&Process->ThreadListHead, &Thread->ThreadListEntry);
-    DPRINT("Thread initalized\n");
 }
 
+VOID
+NTAPI
+KeStartThread(IN OUT PKTHREAD Thread)
+{
+    KIRQL OldIrql;
+    PKPROCESS Process = Thread->ApcState.Process;
+    PKNODE Node;
+    PKPRCB NodePrcb;
+    ULONG Set, Mask;
+    UCHAR IdealProcessor;
+
+    /* Setup static fields from parent */
+    Thread->Iopl = Process->Iopl;
+    Thread->Quantum = Process->QuantumReset;
+    Thread->QuantumReset = Process->QuantumReset;
+    Thread->SystemAffinityActive = FALSE;
+
+    /* Lock the process */
+    KeAcquireSpinLock(&Process->ProcessLock, &OldIrql);
+
+    /* Setup volatile data */
+    Thread->Priority = Process->BasePriority;
+    Thread->BasePriority = Process->BasePriority;
+    Thread->Affinity = Process->Affinity;
+    Thread->UserAffinity = Process->Affinity;
+
+    /* Get the KNODE and its PRCB */
+    Node = KeNodeBlock[Process->IdealNode];
+    NodePrcb = (PKPRCB)(KPCR_BASE + (Process->ThreadSeed * PAGE_SIZE));
+
+    /* Calculate affinity mask */
+    Set = ~NodePrcb->MultiThreadProcessorSet;
+    Mask = (ULONG)(Node->ProcessorMask & Process->Affinity);
+    Set &= Mask;
+    if (Set) Mask = Set;
+
+    /* Get the new thread seed */
+    IdealProcessor = KeFindNextRightSetAffinity(Process->ThreadSeed, Mask);
+    Process->ThreadSeed = IdealProcessor;
+
+    /* Sanity check */
+    ASSERT((Thread->UserAffinity & AFFINITY_MASK(IdealProcessor)));
+
+    /* Set the Ideal Processor */
+    Thread->IdealProcessor = IdealProcessor;
+    Thread->UserIdealProcessor = IdealProcessor;
+
+    /* Lock the Dispatcher Database */
+    KeAcquireDispatcherDatabaseLockAtDpcLevel();
+
+    /* Insert the thread into the process list */
+    InsertTailList(&Process->ThreadListHead, &Thread->ThreadListEntry);
+
+    /* Increase the stack count */
+    ASSERT(Process->StackCount != MAXULONG_PTR);
+    Process->StackCount++;
+
+    /* Release locks and return */
+    KeReleaseDispatcherDatabaseLockFromDpcLevel();
+    KeReleaseSpinLock(&Process->ProcessLock, OldIrql);
+}
+
+VOID
+NTAPI
+KeInitializeThread(IN PKPROCESS Process,
+                   IN OUT PKTHREAD Thread,
+                   IN PKSYSTEM_ROUTINE SystemRoutine,
+                   IN PKSTART_ROUTINE StartRoutine,
+                   IN PVOID StartContext,
+                   IN PCONTEXT Context,
+                   IN PVOID Teb,
+                   IN PVOID KernelStack)
+{
+    /* Initailize and start the thread on success */
+    if (NT_SUCCESS(KeInitThread(Thread,
+                                KernelStack,
+                                SystemRoutine,
+                                StartRoutine,
+                                StartContext,
+                                Context,
+                                Teb,
+                                Process)))
+    {
+        /* Start it */
+        KeStartThread(Thread);
+    }
+}
 
 /*
  * @implemented

reactos/ntoskrnl/ke/process.c

@@ -113,13 +113,14 @@ KiAttachProcess(PKTHREAD Thread,
 
 VOID
 NTAPI
-KeInitializeProcess(PKPROCESS Process,
+KeInitializeProcess(IN OUT PKPROCESS Process,
-                    KPRIORITY Priority,
+                    IN KPRIORITY Priority,
-                    KAFFINITY Affinity,
+                    IN KAFFINITY Affinity,
-                    LARGE_INTEGER DirectoryTableBase)
+                    IN LARGE_INTEGER DirectoryTableBase)
 {
-    DPRINT("KeInitializeProcess. Process: %x, DirectoryTableBase: %x\n",
+    ULONG i = 0;
-            Process, DirectoryTableBase);
+    UCHAR IdealNode = 0;
+    PKNODE Node;
 
     /* Initialize the Dispatcher Header */
     KeInitializeDispatcherHeader(&Process->Header,
@@ -127,19 +128,58 @@ KeInitializeProcess(PKPROCESS Process,
                                  sizeof(KPROCESS),
                                  FALSE);
 
-    /* Initialize Scheduler Data, Disable Alignment Faults and Set the PDE */
+    /* Initialize Scheduler Data, Alignment Faults and Set the PDE */
     Process->Affinity = Affinity;
-    Process->BasePriority = Priority;
+    Process->BasePriority = (CHAR)Priority;
     Process->QuantumReset = 6;
     Process->DirectoryTableBase = DirectoryTableBase;
     Process->AutoAlignment = TRUE;
     Process->IopmOffset = 0xFFFF;
+
+    /* Initialize the lists */
+    InitializeListHead(&Process->ThreadListHead);
+    InitializeListHead(&Process->ProfileListHead);
+    InitializeListHead(&Process->ReadyListHead);
+
+    /* Initialize the current State */
     Process->State = ProcessInMemory;
 
-    /* Initialize the Thread List */
+    /* Check how many Nodes there are on the system */
-    InitializeListHead(&Process->ThreadListHead);
+    if (KeNumberNodes > 1)
-    KeInitializeSpinLock(&Process->ProcessLock);
+    {
-    DPRINT("The Process has now been initalized with the Kernel\n");
+        /* Set the new seed */
+        KeProcessNodeSeed = (KeProcessNodeSeed + 1) / KeNumberNodes;
+        IdealNode = KeProcessNodeSeed;
+
+        /* Loop every node */
+        do
+        {
+            /* Check if the affinity matches */
+            if (KeNodeBlock[IdealNode]->ProcessorMask != Affinity) break;
+
+            /* No match, try next Ideal Node and increase node loop index */
+            IdealNode++;
+            i++;
+
+            /* Check if the Ideal Node is beyond the total number of nodes */
+            if (IdealNode >= KeNumberNodes)
+            {
+                /* Normalize the Ideal Node */
+                IdealNode -= KeNumberNodes;
+            }
+        } while (i < KeNumberNodes);
+    }
+
+    /* Set the ideal node and get the ideal node block */
+    Process->IdealNode = IdealNode;
+    Node = KeNodeBlock[IdealNode];
+    ASSERT(Node->ProcessorMask & Affinity);
+
+    /* Find the matching affinity set to calculate the thread seed */
+    Affinity &= Node->ProcessorMask;
+    Process->ThreadSeed = KeFindNextRightSetAffinity(Node->Seed,
+                                                     (ULONG)Affinity);
+    Node->Seed = Process->ThreadSeed;
 }
 
 ULONG

reactos/ntoskrnl/ps/thread.c

@@ -15,7 +15,6 @@
  *  - MAJOR: Use Guarded Mutex instead of Fast Mutex for Active Process Locks.
  *  - Generate process cookie for user-more thread.
  *  - Add security calls where necessary.
- *  - KeInit/StartThread for better isolation of code
  */
 
 /* INCLUDES ****************************************************************/
@@ -56,7 +55,7 @@ PspUserThreadStartup(PKSTART_ROUTINE StartRoutine,
     {
         /* Remember that we're dead */
         DeadThread = TRUE;
-}
+    }
     else
     {
         /* Get the Locale ID and save Preferred Proc */
@@ -157,12 +156,11 @@ PspCreateThread(OUT PHANDLE ThreadHandle,
     HANDLE hThread;
     PEPROCESS Process;
     PETHREAD Thread;
-    PTEB TebBase;
+    PTEB TebBase = NULL;
     KIRQL OldIrql;
     KPROCESSOR_MODE PreviousMode = ExGetPreviousMode();
     NTSTATUS Status;
     HANDLE_TABLE_ENTRY CidEntry;
-    ULONG_PTR KernelStack;
     PAGED_CODE();
 
     /* If we were called from PsCreateSystemThread, then we're kernel mode */
@@ -260,9 +258,6 @@ PspCreateThread(OUT PHANDLE ThreadHandle,
     /* Acquire rundown protection */
     ExAcquireRundownProtection(&Process->RundownProtect);
 
-    /* Allocate Stack for non-GUI Thread */
-    KernelStack = (ULONG_PTR)MmCreateKernelStack(FALSE) + KERNEL_STACK_SIZE;
-
     /* Now let the kernel initialize the context */
     if (ThreadContext)
     {
@@ -281,14 +276,14 @@ PspCreateThread(OUT PHANDLE ThreadHandle,
         Thread->Win32StartAddress = (PVOID)ThreadContext->Eax;
 
         /* Let the kernel intialize the Thread */
-        KeInitializeThread(&Process->Pcb,
+        Status = KeInitThread(&Thread->Tcb,
-                           &Thread->Tcb,
+                              NULL,
-                           PspUserThreadStartup,
+                              PspUserThreadStartup,
-                           NULL,
+                              NULL,
-                           NULL,
+                              Thread->StartAddress,
-                           ThreadContext,
+                              ThreadContext,
-                           TebBase,
+                              TebBase,
-                           (PVOID)KernelStack);
+                              &Process->Pcb);
     }
     else
     {
@@ -297,16 +292,30 @@ PspCreateThread(OUT PHANDLE ThreadHandle,
         InterlockedOr((PLONG)&Thread->CrossThreadFlags, CT_SYSTEM_THREAD_BIT);
 
         /* Let the kernel intialize the Thread */
-        KeInitializeThread(&Process->Pcb,
+        Status = KeInitThread(&Thread->Tcb,
-                           &Thread->Tcb,
+                              NULL,
-                           PspSystemThreadStartup,
+                              PspSystemThreadStartup,
-                           StartRoutine,
+                              StartRoutine,
-                           StartContext,
+                              StartContext,
-                           NULL,
+                              NULL,
-                           NULL,
+                              NULL,
-                           (PVOID)KernelStack);
+                              &Process->Pcb);
     }
 
+    /* Check if we failed */
+    if (!NT_SUCCESS(Status))
+    {
+        /* Delete the TEB if we had done */
+        if (TebBase) MmDeleteTeb(Process, TebBase);
+
+        /* Release rundown and dereference */
+        ExReleaseRundownProtection(&Process->RundownProtect);
+        ObDereferenceObject(Thread);
+        return Status;
+    }
+
+    /* FIXME: Acquire exclusive pushlock */
+
     /*
      * Insert the Thread into the Process's Thread List
      * Note, this is the ETHREAD Thread List. It is removed in
@@ -315,6 +324,11 @@ PspCreateThread(OUT PHANDLE ThreadHandle,
     InsertTailList(&Process->ThreadListHead, &Thread->ThreadListEntry);
     Process->ActiveThreads++;
 
+    /* Start the thread */
+    KeStartThread(&Thread->Tcb);
+
+    /* FIXME: Wake pushlock */
+
     /* Release rundown */
     ExReleaseRundownProtection(&Process->RundownProtect);
 
@@ -326,10 +340,7 @@ PspCreateThread(OUT PHANDLE ThreadHandle,
     PspRunCreateThreadNotifyRoutines(Thread, TRUE);
 
     /* Suspend the Thread if we have to */
-    if (CreateSuspended)
+    if (CreateSuspended) KeSuspendThread(&Thread->Tcb);
-    {
-        KeSuspendThread(&Thread->Tcb);
-    }
 
     /* Check if we were already terminated */
     if (Thread->Terminated)