Implement processor affinity search functions

xtoskrnl/CMakeLists.txt

@@ -38,6 +38,7 @@ list(APPEND XTOSKRNL_SOURCE
     ${XTOSKRNL_SOURCE_DIR}/ke/${ARCH}/krnlinit.cc
     ${XTOSKRNL_SOURCE_DIR}/ke/${ARCH}/kthread.cc
     ${XTOSKRNL_SOURCE_DIR}/ke/${ARCH}/proc.cc
+    ${XTOSKRNL_SOURCE_DIR}/ke/affinity.cc
     ${XTOSKRNL_SOURCE_DIR}/ke/apc.cc
     ${XTOSKRNL_SOURCE_DIR}/ke/bootinfo.cc
     ${XTOSKRNL_SOURCE_DIR}/ke/crash.cc

xtoskrnl/includes/ke.hh

@@ -11,6 +11,7 @@
 
 #include <xtos.hh>
 
+#include <ke/affinity.hh>
 #include <ke/apc.hh>
 #include <ke/bootinfo.hh>
 #include <ke/crash.hh>

xtoskrnl/includes/ke/affinity.hh

@@ -0,0 +1,30 @@
+/**
+ * PROJECT:         ExectOS
+ * COPYRIGHT:       See COPYING.md in the top level directory
+ * FILE:            xtoskrnl/includes/ke/affinity.hh
+ * DESCRIPTION:     XT kernel processor affinity management support
+ * DEVELOPERS:      Aiken Harris <harraiken91@gmail.com>
+ */
+
+#ifndef __XTOSKRNL_KE_AFFINITY_HH
+#define __XTOSKRNL_KE_AFFINITY_HH
+
+#include <xtos.hh>
+
+
+/* Kernel Library */
+namespace KE
+{
+    class Affinity
+    {
+        public:
+            STATIC XTAPI VOID CopyAffinity(OUT PKAFFINITY_MAP Destination,
+                                           IN PKAFFINITY_MAP Source);
+            STATIC XTAPI ULONG FindNextLeftSetProcessor(IN ULONG ThreadSeed,
+                                                        IN PKAFFINITY_MAP AffinityMap);
+            STATIC XTAPI ULONG FindNextRightSetProcessor(IN ULONG ThreadSeed,
+                                                         IN PKAFFINITY_MAP AffinityMap);
+    };
+}
+
+#endif /* __XTOSKRNL_KE_AFFINITY_HH */

xtoskrnl/ke/affinity.cc

@@ -0,0 +1,232 @@
+/**
+ * PROJECT:         ExectOS
+ * COPYRIGHT:       See COPYING.md in the top level directory
+ * FILE:            xtoskrnl/ke/affinity.cc
+ * DESCRIPTION:     XT kernel processor affinity management support
+ * DEVELOPERS:      Aiken Harris <harraiken91@gmail.com>
+ */
+
+#include <xtos.hh>
+
+
+/**
+ * Copies the topological layout and processor bindings from a source affinity map to a destination map.
+ *
+ * @param Destination
+ *        Supplies a pointer to the target affinity map that will receive the copied data.
+ *
+ * @param Source
+ *        Supplies a pointer to the source affinity map containing the active processor bindings.
+ *
+ * @return This routine does not return any value.
+ *
+ * @since XT 1.0
+ */
+XTAPI
+VOID
+KE::Affinity::CopyAffinity(OUT PKAFFINITY_MAP Destination,
+                           IN PKAFFINITY_MAP Source)
+{
+    USHORT Index;
+
+    /* Copy map metadata */
+    Destination->Count = Source->Count;
+    Destination->Size = Source->Size;
+    Destination->Reserved = Source->Reserved;
+
+    /* Copy the active affinity bitmasks */
+    for(Index = 0; Index < Source->Size; Index++)
+    {
+        /* Replicate the hardware topology bindings across all active array elements */
+        Destination->Bitmap[Index] = Source->Bitmap[Index];
+    }
+}
+
+
+/**
+ * Locates the next available logical processor to the left (higher topological index) of a specified seed.
+ *
+ * @param ThreadSeed
+ *        Supplies the logical processor index used as the starting point for the upward search.
+ *
+ * @param AffinityMap
+ *        Supplies a pointer to the extended affinity map defining the permitted processors.
+ *
+ * @return Returns the absolute topological index of the selected processor.
+ *
+ * @since XT 1.0
+ */
+XTAPI
+ULONG
+KE::Affinity::FindNextLeftSetProcessor(IN ULONG ThreadSeed,
+                                       IN PKAFFINITY_MAP AffinityMap)
+{
+    ULONG BitIndex, BitsPerMask, Index, StartBit, StartIndex;
+    KAFFINITY Mask;
+
+    /* Define the architectural bit width of a single affinity mask */
+    BitsPerMask = sizeof(KAFFINITY) * 8;
+
+    /* Prevent division by zero and out-of-bounds access if the topology map is uninitialized or empty */
+    if(AffinityMap->Size == 0)
+    {
+        /* Fallback to the bootstrap processor */
+        return 0;
+    }
+
+    /* Calculate the target array index and bit offset based on the thread seed */
+    StartIndex = (ThreadSeed / BitsPerMask) % AffinityMap->Size;
+    StartBit = ThreadSeed % BitsPerMask;
+
+    /* Isolate the segment of the current affinity mask strictly to the left */
+    if(StartBit == (BitsPerMask - 1))
+    {
+        /* Prevent undefined behavior when shifting by the total architectural bit width */
+        Mask = 0;
+    }
+    else
+    {
+        /* Mask out the seed bit and all bits below it */
+        Mask = AffinityMap->Bitmap[StartIndex] & (~((KAFFINITY)0) << (StartBit + 1));
+    }
+
+    /* Evaluate if any allowed processors exist in the higher portion of the current mask */
+    if(Mask != 0)
+    {
+        /* Locate the rightmost set bit within this masked subset */
+        if(AR::CpuFunctions::ScanForwardBit(&BitIndex, Mask))
+        {
+            /* Return the absolute topological index of the located processor */
+            return (StartIndex * BitsPerMask) + BitIndex;
+        }
+    }
+
+    /* Ascend through the subsequent array elements in the map */
+    for(Index = StartIndex + 1; Index < AffinityMap->Size; Index++)
+    {
+        /* Load the complete processor mask for the current array boundary */
+        Mask = AffinityMap->Bitmap[Index];
+
+        /* Check if this segment contains any active processor bindings */
+        if(Mask != 0)
+        {
+            /* Find the lowest available processor within this array element */
+            if(AR::CpuFunctions::ScanForwardBit(&BitIndex, Mask))
+            {
+                /* Return the absolute topological index of the located processor */
+                return (Index * BitsPerMask) + BitIndex;
+            }
+        }
+    }
+
+    /* Wrap around and scan the entire map from the bottom to find the lowest globally permitted processor */
+    for(Index = 0; Index < AffinityMap->Size; Index++)
+    {
+        /* Load the complete processor mask for the current array boundary */
+        Mask = AffinityMap->Bitmap[Index];
+
+        /* Check if this segment contains any active processor bindings */
+        if(Mask != 0)
+        {
+            /* Find the lowest available processor within this array element */
+            if(AR::CpuFunctions::ScanForwardBit(&BitIndex, Mask))
+            {
+                /* Return the absolute topological index of the located processor */
+                return (Index * BitsPerMask) + BitIndex;
+            }
+        }
+    }
+
+    /* Fallback to the bootstrap processor */
+    return 0;
+}
+
+/**
+ * Locates the next available logical processor to the right (lower topological index) of a specified seed.
+ *
+ * @param ThreadSeed
+ *        Supplies the logical processor index used as the starting point for the downward search.
+ *
+ * @param AffinityMap
+ *        Supplies a pointer to the extended affinity map defining the permitted processors.
+ *
+ * @return Returns the absolute topological index of the selected processor.
+ *
+ * @since XT 1.0
+ */
+XTAPI
+ULONG
+KE::Affinity::FindNextRightSetProcessor(IN ULONG ThreadSeed,
+                                        IN PKAFFINITY_MAP AffinityMap)
+{
+    ULONG BitIndex, BitsPerMask, StartBit, StartIndex;
+    KAFFINITY Mask;
+    LONG Index;
+
+    /* Define the architectural bit width of a single affinity mask */
+    BitsPerMask = sizeof(KAFFINITY) * 8;
+
+    /* Prevent division by zero and out-of-bounds access if the topology map is uninitialized or empty */
+    if(AffinityMap->Size == 0)
+    {
+        /* Fallback to the bootstrap processor */
+        return 0;
+    }
+
+    /* Calculate the target array index and bit offset based on the seed */
+    StartIndex = (ThreadSeed / BitsPerMask) % AffinityMap->Size;
+    StartBit = ThreadSeed % BitsPerMask;
+
+    /* Isolate the segment of the current affinity mask strictly to the right */
+    Mask = AffinityMap->Bitmap[StartIndex] & (((KAFFINITY)1 << StartBit) - 1);
+
+    /* Evaluate if any allowed processors exist in the lower portion of the current mask */
+    if(Mask != 0)
+    {
+        /* Locate the leftmost set bit within this masked subset */
+        if(AR::CpuFunctions::ScanReverseBit(&BitIndex, Mask))
+        {
+            /* Return the absolute topological index of the located processor */
+            return (StartIndex * BitsPerMask) + BitIndex;
+        }
+    }
+
+    /* Descend through the preceding array elements in the map */
+    for(Index = (LONG)StartIndex - 1; Index >= 0; Index--)
+    {
+        /* Load the complete processor mask for the current array boundary */
+        Mask = AffinityMap->Bitmap[Index];
+
+        /* Check if this segment contains any active processor bindings */
+        if(Mask != 0)
+        {
+            /* Find the highest available processor within this array element */
+            if(AR::CpuFunctions::ScanReverseBit(&BitIndex, Mask))
+            {
+                /* Return the absolute topological index of the located processor */
+                return ((ULONG)Index * BitsPerMask) + BitIndex;
+            }
+        }
+    }
+
+    /* Wrap around and scan the entire map from the top to find the highest globally permitted processor */
+    for(Index = (LONG)AffinityMap->Size - 1; Index >= 0; Index--)
+    {
+        /* Load the complete processor mask for the current array boundary */
+        Mask = AffinityMap->Bitmap[Index];
+
+        /* Check if this segment contains any active processor bindings */
+        if(Mask != 0)
+        {
+            /* Find the highest available processor within this array element */
+            if(AR::CpuFunctions::ScanReverseBit(&BitIndex, Mask))
+            {
+                /* Return the absolute topological index of the located processor */
+                return ((ULONG)Index * BitsPerMask) + BitIndex;
+            }
+        }
+    }
+
+    /* Fallback to the bootstrap processor */
+    return 0;
+}