Index: src/system/kernel/arch/x86/arch_cpu_specific.h
===================================================================
--- src/system/kernel/arch/x86/arch_cpu_specific.h	(revision 0)
+++ src/system/kernel/arch/x86/arch_cpu_specific.h	(revision 0)
@@ -0,0 +1,156 @@
+/*
+ * Copyright 2011, James Dewey Taylor, james.dewey.taylor@gmail.com
+ * Distributed under the terms of the MIT license.
+ */
+#ifndef ARCH_CPU_SPECIFIC_H
+#define ARCH_CPU_SPECIFIC_H
+
+#include <cpu.h>
+
+
+status_t get_topology_intel(cpu_ent*);
+status_t get_topology_amd(cpu_ent*);
+
+inline uint64
+next_power_of_two(uint32 in)
+{
+	if (in > 2147483648UL)
+		return 4294967296ULL;
+	else if (in > 1073741824U)
+		return 2147483648UL;
+	else if (in > 536870912)
+		return 1073741824;
+	else if (in > 268435456)
+		return 536870912;
+	else if (in > 134217728)
+		return 268435456;
+	else if (in > 67108864)
+		return 134217728;
+	else if (in > 33554432)
+		return 67108864;
+	else if (in > 16777216)
+		return 33554432;
+	else if (in > 8388608)
+		return 16777216;
+	else if (in > 4194304)
+		return 8388608;
+	else if (in > 2097152)
+		return 4194304;
+	else if (in > 1048576)
+		return 2097152;
+	else if (in > 524288)
+		return 1048576;
+	else if (in > 262144)
+		return 524288;
+	else if (in > 131072)
+		return 262144;
+	else if (in > 65536)
+		return 131072;
+	else if (in > 32768)
+		return 65536;
+	else if (in > 16384)
+		return 32768;
+	else if (in > 8192)
+		return 16384;
+	else if (in > 4096)
+		return 8192;
+	else if (in > 2048)
+		return 4096;
+	else if (in > 2048)
+		return 4096;
+	else if (in > 1024)
+		return 2048;
+	else if (in > 512)
+		return 1024;
+	else if (in > 256)
+		return 512;
+	else if (in > 128)
+		return 256;
+	else if (in > 64)
+		return 128;
+	else if (in > 32)
+		return 64;
+	else if (in > 16)
+		return 32;
+	else if (in > 8)
+		return 16;
+	else if (in > 4)
+		return 8;
+	else if (in > 2)
+		return 4;
+	else
+		return in;
+}
+
+
+inline uint32
+log2(uint32 in)
+{
+	if (in == 536870912)
+		return 31;
+	else if (in == 268435456)
+		return 30;
+	else if (in == 134217728)
+		return 29;
+	else if (in == 67108864)
+		return 28;
+	else if (in == 33554432)
+		return 27;
+	else if (in == 16777216)
+		return 26;
+	else if (in == 8388608)
+		return 25;
+	else if (in == 4194304)
+		return 24;
+	else if (in == 2097152)
+		return 23;
+	else if (in == 1048576)
+		return 22;
+	else if (in == 524288)
+		return 21;
+	else if (in == 262144)
+		return 20;
+	else if (in == 131072)
+		return 19;
+	else if (in == 65536)
+		return 18;
+	else if (in == 32768)
+		return 17;
+	else if (in == 16384)
+		return 16;
+	else if (in == 8192)
+		return 15;
+	else if (in == 4096)
+		return 14;
+	else if (in == 2048)
+		return 13;
+	else if (in == 2048)
+		return 12;
+	else if (in == 1024)
+		return 11;
+	else if (in == 512)
+		return 10;
+	else if (in == 256)
+		return 9;
+	else if (in == 128)
+		return 8;
+	else if (in == 64)
+		return 7;
+	else if (in == 32)
+		return 6;
+	else if (in == 16)
+		return 5;
+	else if (in == 8)
+		return 4;
+	else if (in == 4)
+		return 3;
+	else if (in == 2)
+		return 2;
+	else if (in == 1)
+		return 1;
+	else
+		return 0;
+}
+
+
+#endif // ARCH_CPU_SPECIFIC_H
Index: src/system/kernel/arch/x86/arch_cpu_intel.cpp
===================================================================
--- src/system/kernel/arch/x86/arch_cpu_intel.cpp	(revision 0)
+++ src/system/kernel/arch/x86/arch_cpu_intel.cpp	(revision 0)
@@ -0,0 +1,60 @@
+/*
+ * Copyright 2011, James Dewey Taylor, james.dewey.taylor@gmail.com
+ * Distributed under the terms of the MIT license.
+ */
+
+
+#include <arch_system_info.h>
+#include <apic.h>
+
+#include "arch_cpu_specific.h"
+
+status_t
+get_topology_intel(cpu_ent* cpu)
+{
+	cpuid_info cpuid;
+
+	uint32 logicalCPUBits = 0;
+	uint32 coreBits = 0;
+
+	cpu->has_topology_data = false;
+
+	// We will NOT use cpuid leaf 11 because x2APIC isn't enabled, so use
+	// leaves 1 & 4 if they're available, even if leaf 11 is also available
+	get_current_cpuid(&cpuid, 0);
+	if (cpuid.regs.eax >= 4) {
+		// do leaf 1 & 4 crap...
+		uint32 apicID = apic_local_id();
+		get_current_cpuid(&cpuid, 1);
+		uint32 temp1 = (cpuid.regs.ebx >> 16) & 255;
+		get_current_cpuid(&cpuid, 4, 0);
+		uint32 temp2 = cpuid.regs.eax >> 26;
+		logicalCPUBits = log2(next_power_of_two(temp1) / (temp2 + 1));
+		coreBits = log2(temp2);
+
+		cpu->cpu_num_in_core = apicID & ((1 << logicalCPUBits) -1);
+		apicID = apicID >> logicalCPUBits;
+		cpu->core_num = apicID & ((1 << coreBits) -1);
+		apicID = apicID >> coreBits;
+		cpu->package_num = apicID;
+		cpu->has_topology_data = true;
+
+		return B_OK;
+	} else {
+		get_current_cpuid(&cpuid, 1);
+		if ((cpuid.regs.ebx & 0xFF0000) > 0) {
+			// 2 level topology
+			// 2 level topologies will not have HT/SMT enabled, so ultimately
+			// they don't matter
+			// --- I have no clue how to get this without leaves 1 & 4
+			cpu->package_num = cpu->cpu_num;
+			return B_OK;
+		} else {
+			// 1 level topology
+			// 1 level topologies are a 1:1 relationship between packages and
+			// logical cpu ids... no HT/SMT, so they don't matter either
+			cpu->package_num = cpu->cpu_num;
+			return B_OK;
+		}
+	}
+}
Index: src/system/kernel/arch/x86/Jamfile
===================================================================
--- src/system/kernel/arch/x86/Jamfile	(revision 43175)
+++ src/system/kernel/arch/x86/Jamfile	(working copy)
@@ -19,6 +19,8 @@
 KernelMergeObject kernel_arch_x86.o :
 	arch_commpage.cpp
 	arch_cpu.cpp
+	arch_cpu_amd.cpp
+	arch_cpu_intel.cpp
 	arch_debug.cpp
 	arch_debug_console.cpp
 	arch_elf.cpp
Index: src/system/kernel/arch/x86/arch_cpu_amd.cpp
===================================================================
--- src/system/kernel/arch/x86/arch_cpu_amd.cpp	(revision 0)
+++ src/system/kernel/arch/x86/arch_cpu_amd.cpp	(revision 0)
@@ -0,0 +1,17 @@
+/*
+ * Copyright 2011, James Dewey Taylor, james.dewey.taylor@gmail.com
+ * Distributed under the terms of the MIT license.
+ */
+
+#include "arch_cpu_specific.h"
+
+
+status_t
+get_topology_amd(cpu_ent* cpu)
+{
+	//uint32 logicalCPUBits = 0;
+	//uint32 coreBits = 0;
+
+	// TODO: return true by default?
+	return !B_OK;
+}
Index: src/system/kernel/arch/x86/cpuid.S
===================================================================
--- src/system/kernel/arch/x86/cpuid.S	(revision 43175)
+++ src/system/kernel/arch/x86/cpuid.S	(working copy)
@@ -11,12 +11,14 @@
 
 .text
 
-/* void get_current_cpuid(cpuid_info *info, uint32 eaxRegister) */
+/* status_t get_current_cpuid(cpuid_info *info, uint32 eaxRegister,
+uint32 ecxRegister = 0) */
 FUNCTION(get_current_cpuid):
  	pushl	%ebx
  	pushl	%edi
  	movl	12(%esp),%edi	/* first arg points to the cpuid_info structure */
  	movl	16(%esp),%eax	/* second arg sets up eax */
+ 	movl	20(%esp),%ecx	/* third arg sets up ecx */
  	cpuid
  	movl	%eax,0(%edi)	/* copy the regs into the cpuid_info structure */
  	movl	%ebx,4(%edi)
Index: src/system/kernel/arch/x86/arch_smp.cpp
===================================================================
--- src/system/kernel/arch/x86/arch_smp.cpp	(revision 43175)
+++ src/system/kernel/arch/x86/arch_smp.cpp	(working copy)
@@ -26,6 +26,8 @@
 #include <stdio.h>
 
 
+#include "arch_cpu_specific.h"
+
 //#define TRACE_ARCH_SMP
 #ifdef TRACE_ARCH_SMP
 #	define TRACE(x) dprintf x
@@ -109,6 +111,21 @@
 
 	init_sse();
 
+
+	// determine processor topology for scheduler
+	if (gCPU[cpu].arch.vendor == VENDOR_INTEL) {
+		if (get_topology_intel(&gCPU[cpu]) != B_OK) {
+			panic("detect_cpu(): unable to get topology for Intel chip\n");
+		}
+	} else if (gCPU[cpu].arch.vendor == VENDOR_AMD) {
+		if (get_topology_amd(&gCPU[cpu]) != B_OK) {
+			panic("detect_cpu(): unable to get topology for AMD chip\n");
+		}
+	} else { // Only Intel and AMD vendors currently supported.
+		gCPU[cpu].has_topology_data = false;
+	}
+
+
 	return B_OK;
 }
 
Index: src/system/kernel/team.cpp
===================================================================
--- src/system/kernel/team.cpp	(revision 43175)
+++ src/system/kernel/team.cpp	(working copy)
@@ -447,6 +447,9 @@
 	user_data_size = 0;
 	free_user_threads = NULL;
 
+	// new team has no soft affinity
+	preferred_cpu = -1;
+
 	supplementary_groups = NULL;
 	supplementary_group_count = 0;
 
Index: src/system/kernel/scheduler/scheduler.cpp
===================================================================
--- src/system/kernel/scheduler/scheduler.cpp	(revision 43175)
+++ src/system/kernel/scheduler/scheduler.cpp	(working copy)
@@ -68,7 +68,7 @@
 		cpuCount != 1 ? "s" : "");
 
 	if (cpuCount > 1) {
-#if 0
+#if 1
 		dprintf("scheduler_init: using affine scheduler\n");
 		scheduler_affine_init();
 #else
Index: src/system/kernel/scheduler/scheduler_affine.cpp
===================================================================
--- src/system/kernel/scheduler/scheduler_affine.cpp	(revision 43175)
+++ src/system/kernel/scheduler/scheduler_affine.cpp	(working copy)
@@ -1,4 +1,5 @@
 /*
+ * Copyright 2011, James Dewey Taylor, james.dewey.taylor@gmail.com
  * Copyright 2009, Rene Gollent, rene@gollent.com.
  * Copyright 2008-2011, Ingo Weinhold, ingo_weinhold@gmx.de.
  * Copyright 2002-2010, Axel Dörfler, axeld@pinc-software.de.
@@ -37,11 +38,16 @@
 #	define TRACE(x) ;
 #endif
 
+// Helper macros
+#define RUN_QUEUE(x) sRunQueue[sCPUMap[x]]
+#define RUN_QUEUE_SIZE(x) sRunQueueSize[sCPUMap[x]]
+
 // The run queues. Holds the threads ready to run ordered by priority.
 // One queue per schedulable target (CPU, core, etc.).
 // TODO: consolidate this such that HT/SMT entities on the same physical core
 // share a queue, once we have the necessary API for retrieving the topology
 // information
+static int32 sCPUMap[B_MAX_CPU_COUNT];
 static Thread* sRunQueue[B_MAX_CPU_COUNT];
 static int32 sRunQueueSize[B_MAX_CPU_COUNT];
 static Thread* sIdleThreads;
@@ -108,10 +114,10 @@
 	Thread *thread = NULL;
 
 	for (int32 i = 0; i < smp_get_num_cpus(); i++) {
-		thread = sRunQueue[i];
+		thread = RUN_QUEUE(i);
 		kprintf("Run queue for cpu %ld (%ld threads)\n", i,
-			sRunQueueSize[i]);
-		if (sRunQueueSize[i] > 0) {
+			RUN_QUEUE_SIZE(i));
+		if (RUN_QUEUE_SIZE(i) > 0) {
 			kprintf("thread      id      priority  avg. quantum  name\n");
 			while (thread) {
 				kprintf("%p  %-7ld %-8ld  %-12ld  %s\n", thread, thread->id,
@@ -126,6 +132,20 @@
 }
 
 
+static int
+dump_topology(int argc, char **argv)
+{
+	kprintf("Processor Topology Data\n");
+	kprintf("Num\tCPU\tNumOnCore\tCore\tPackage\n");
+	for (int32 i = 0; i < smp_get_num_cpus(); i++) {
+		kprintf("%ld\t%d\t:\t%d\t%d\t%d\n",
+			i, gCPU[i].cpu_num, gCPU[i].cpu_num_in_core, gCPU[i].core_num,
+			gCPU[i].package_num);
+	}
+	return 0;
+}
+
+
 /*!	Returns the most idle CPU based on the active time counters.
 	Note: thread lock must be held when entering this function
 */
@@ -136,7 +156,7 @@
 	for (int32 i = 0; i < smp_get_num_cpus(); i++) {
 		if (gCPU[i].disabled)
 			continue;
-		if (targetCPU < 0 || sRunQueueSize[i] < sRunQueueSize[targetCPU])
+		if (targetCPU < 0 || RUN_QUEUE_SIZE(i) < RUN_QUEUE_SIZE(targetCPU))
 			targetCPU = i;
 	}
 
@@ -153,9 +173,11 @@
 	int32 targetCPU = -1;
 	if (thread->pinned_to_cpu > 0)
 		targetCPU = thread->previous_cpu->cpu_num;
-	else if (thread->previous_cpu == NULL || thread->previous_cpu->disabled)
-		targetCPU = affine_get_most_idle_cpu();
-	else
+	else if (thread->previous_cpu == NULL || thread->previous_cpu->disabled) {
+		if (thread->team->preferred_cpu < 0)
+			thread->team->preferred_cpu = affine_get_most_idle_cpu();
+		targetCPU = thread->team->preferred_cpu;
+	} else
 		targetCPU = thread->previous_cpu->cpu_num;
 
 	thread->state = thread->next_state = B_THREAD_READY;
@@ -165,22 +187,22 @@
 		sIdleThreads = thread;
 	} else {
 		Thread *curr, *prev;
-		for (curr = sRunQueue[targetCPU], prev = NULL; curr
+		for (curr = RUN_QUEUE(targetCPU), prev = NULL; curr
 			&& curr->priority >= thread->next_priority;
 			curr = curr->queue_next) {
 			if (prev)
 				prev = prev->queue_next;
 			else
-				prev = sRunQueue[targetCPU];
+				prev = RUN_QUEUE(targetCPU);
 		}
 
 		T(EnqueueThread(thread, prev, curr));
-		sRunQueueSize[targetCPU]++;
+		RUN_QUEUE_SIZE(targetCPU)++;
 		thread->queue_next = curr;
 		if (prev)
 			prev->queue_next = thread;
 		else
-			sRunQueue[targetCPU] = thread;
+			RUN_QUEUE(targetCPU) = thread;
 
 		thread->scheduler_data->fLastQueue = targetCPU;
 	}
@@ -213,10 +235,10 @@
 		resultThread = prevThread->queue_next;
 		prevThread->queue_next = resultThread->queue_next;
 	} else {
-		resultThread = sRunQueue[currentCPU];
-		sRunQueue[currentCPU] = resultThread->queue_next;
+		resultThread = RUN_QUEUE(currentCPU);
+		RUN_QUEUE(currentCPU) = resultThread->queue_next;
 	}
-	sRunQueueSize[currentCPU]--;
+	RUN_QUEUE_SIZE(currentCPU)--;
 	resultThread->scheduler_data->fLastQueue = -1;
 
 	return resultThread;
@@ -239,22 +261,22 @@
 	int32 targetCPU = -1;
 	for (int32 i = 0; i < smp_get_num_cpus(); i++) {
 		// skip CPUs that have either no or only one thread
-		if (i == currentCPU || sRunQueueSize[i] < 2)
+		if (i == currentCPU || RUN_QUEUE_SIZE(i) < 2)
 			continue;
 
 		// out of the CPUs with threads available to steal,
 		// pick whichever one is generally the most CPU bound.
 		if (targetCPU < 0
-			|| sRunQueue[i]->priority > sRunQueue[targetCPU]->priority
-			|| (sRunQueue[i]->priority == sRunQueue[targetCPU]->priority
-				&& sRunQueueSize[i] > sRunQueueSize[targetCPU]))
+			|| RUN_QUEUE(i)->priority > RUN_QUEUE(targetCPU)->priority
+			|| (RUN_QUEUE(i)->priority == RUN_QUEUE(targetCPU)->priority
+				&& RUN_QUEUE_SIZE(i) > RUN_QUEUE_SIZE(targetCPU)))
 			targetCPU = i;
 	}
 
 	if (targetCPU < 0)
 		return NULL;
 
-	Thread* nextThread = sRunQueue[targetCPU];
+	Thread* nextThread = RUN_QUEUE(targetCPU);
 	Thread* prevThread = NULL;
 
 	while (nextThread != NULL) {
@@ -302,7 +324,7 @@
 	Thread *item = NULL, *prev = NULL;
 	targetCPU = thread->scheduler_data->fLastQueue;
 
-	for (item = sRunQueue[targetCPU], prev = NULL; item && item != thread;
+	for (item = RUN_QUEUE(targetCPU), prev = NULL; item && item != thread;
 			item = item->queue_next) {
 		if (prev)
 			prev = prev->queue_next;
@@ -392,10 +414,10 @@
 			break;
 	}
 
-	nextThread = sRunQueue[currentCPU];
+	nextThread = RUN_QUEUE(currentCPU);
 	prevThread = NULL;
 
-	if (sRunQueue[currentCPU] != NULL) {
+	if (RUN_QUEUE(currentCPU) != NULL) {
 		TRACE(("dequeueing next thread from cpu %ld\n", currentCPU));
 		// select next thread from the run queue
 		while (nextThread->queue_next) {
@@ -574,4 +596,27 @@
 	memset(sRunQueueSize, 0, sizeof(sRunQueueSize));
 	add_debugger_command_etc("run_queue", &dump_run_queue,
 		"List threads in run queue", "\nLists threads in run queue", 0);
+	add_debugger_command_etc("topology", &dump_topology,
+		"List processor topology", "\nLists processor topology", 0);
+	// TODO: get topology info to initialize sCPUMap
+	// we're assuming a homogenous topology for now
+	// also we're just worried about HT not various levels of cache sharing
+	if (gCPU[0].has_topology_data) {
+		int maxcorenum = 0;
+		for (int i = 0; i < smp_get_num_cpus(); i++) {
+			if (gCPU[i].core_num > maxcorenum)
+				maxcorenum = gCPU[i].core_num;
+		}
+		for (int i = 0; i < smp_get_num_cpus(); i++) {
+			sCPUMap[i] = (maxcorenum + 1) * gCPU[i].package_num +
+				gCPU[i].core_num;
+		}
+	} else {
+		for (int i = 0; i < B_MAX_CPU_COUNT ; i++) {
+			sCPUMap[i] = i;
+		}
+	}
+#if 0
+	//dump_topology(0, NULL);
+#endif
 }
Index: headers/private/kernel/arch/x86/arch_system_info.h
===================================================================
--- headers/private/kernel/arch/x86/arch_system_info.h	(revision 43175)
+++ headers/private/kernel/arch/x86/arch_system_info.h	(working copy)
@@ -12,7 +12,7 @@
 extern "C" {
 #endif
 
-status_t get_current_cpuid(cpuid_info *info, uint32 eax);
+status_t get_current_cpuid(cpuid_info* info, uint32 eax, uint32 ecx = 0);
 uint32 get_eflags(void);
 void set_eflags(uint32 value);
 
Index: headers/private/kernel/cpu.h
===================================================================
--- headers/private/kernel/cpu.h	(revision 43175)
+++ headers/private/kernel/cpu.h	(working copy)
@@ -34,8 +34,16 @@
 /* CPU local data structure */
 
 typedef struct cpu_ent {
+	// the logical cpu id
 	int				cpu_num;
 
+	// the physical location of the logical cpu
+	int				cpu_num_in_core;
+	int				core_num;
+	int				package_num;
+
+	bool			has_topology_data;
+
 	// thread.c: used to force a reschedule at quantum expiration time
 	int				preempted;
 	timer			quantum_timer;
Index: headers/private/kernel/thread_types.h
===================================================================
--- headers/private/kernel/thread_types.h	(revision 43175)
+++ headers/private/kernel/thread_types.h	(working copy)
@@ -236,6 +236,10 @@
 	struct list		dead_threads;
 	int				dead_threads_count;
 
+	int32			preferred_cpu;	// soft affinity for the team (can be
+									// overridden by setting a thread's hard
+									// affinity
+
 	// protected by the team's fLock
 	team_dead_children dead_children;
 	team_job_control_children stopped_children;
