From 343c4896892733387f1e1b1ed01f151107e7da5f Mon Sep 17 00:00:00 2001
From: Pawel Dziepak <pdziepak@quarnos.org>
Date: Mon, 21 Oct 2013 01:33:35 +0200
Subject: [PATCH] kernel: Create CPU topology tree

---
 headers/private/kernel/cpu.h |  12 ++++
 src/system/kernel/cpu.cpp    | 111 +++++++++++++++++++++++++++++++++++
 src/system/kernel/main.cpp   |   1 +
 3 files changed, 124 insertions(+)

diff --git a/headers/private/kernel/cpu.h b/headers/private/kernel/cpu.h
index e8fe6bc490..d3cf0a8bcd 100644
--- a/headers/private/kernel/cpu.h
+++ b/headers/private/kernel/cpu.h
@@ -39,6 +39,15 @@ typedef enum cpu_topology_level {
 	CPU_TOPOLOGY_LEVELS
 } cpu_topology_level;
 
+typedef struct cpu_topology_node {
+	cpu_topology_level	level;
+
+	int					id;
+
+	cpu_topology_node**	children;
+	int					children_count;
+} cpu_topology_node;
+
 
 /* CPU local data structure */
 
@@ -93,6 +102,9 @@ bigtime_t cpu_get_active_time(int32 cpu);
 cpu_ent *get_cpu_struct(void);
 extern inline cpu_ent *get_cpu_struct(void) { return &gCPU[smp_get_current_cpu()]; }
 
+status_t cpu_build_topology_tree(void);
+cpu_topology_node* get_cpu_topology(void);
+
 void _user_clear_caches(void *address, size_t length, uint32 flags);
 bool _user_cpu_enabled(int32 cpu);
 status_t _user_set_cpu_enabled(int32 cpu, bool enabled);
diff --git a/src/system/kernel/cpu.cpp b/src/system/kernel/cpu.cpp
index 1dfbc4dcc1..36df9aa7cc 100644
--- a/src/system/kernel/cpu.cpp
+++ b/src/system/kernel/cpu.cpp
@@ -21,7 +21,9 @@
 
 /* global per-cpu structure */
 cpu_ent gCPU[MAX_BOOT_CPUS];
+
 uint32 gCPUCacheLevelCount;
+static cpu_topology_node sCPUTopology;
 
 static spinlock sSetCpuLock;
 
@@ -88,6 +90,115 @@ clear_caches(void *address, size_t length, uint32 flags)
 }
 
 
+static status_t
+cpu_create_topology_node(cpu_topology_node* node, int32* maxID, int32 id)
+{
+	cpu_topology_level level = static_cast<cpu_topology_level>(node->level - 1);
+	ASSERT(level >= 0);
+
+	cpu_topology_node* newNode = new(std::nothrow) cpu_topology_node;
+	if (newNode == NULL)
+		return B_NO_MEMORY;
+	node->children[id] = newNode;
+
+	newNode->level = level;
+	if (level != CPU_TOPOLOGY_SMT) {
+		newNode->children_count = maxID[level - 1];
+		newNode->children
+			= new(std::nothrow) cpu_topology_node*[maxID[level - 1]];
+		if (newNode->children == NULL)
+			return B_NO_MEMORY;
+
+		memset(newNode->children, 0,
+			maxID[level - 1] * sizeof(cpu_topology_node*));
+	} else {
+		newNode->children_count = 0;
+		newNode->children = NULL;
+	}
+
+	return B_OK;
+}
+
+
+static void
+cpu_rebuild_topology_tree(cpu_topology_node* node, int32* lastID)
+{
+	if (node->children == NULL)
+		return;
+
+	int32 count = 0;
+	for (int32 i = 0; i < node->children_count; i++) {
+		if (node->children[i] == NULL)
+			continue;
+
+		if (count != i)
+			node->children[count] = node->children[i];
+
+		if (node->children[count]->level != CPU_TOPOLOGY_SMT)
+			node->children[count]->id = lastID[node->children[count]->level]++;
+
+		cpu_rebuild_topology_tree(node->children[count], lastID);
+		count++;
+	}
+	node->children_count = count;
+}
+
+
+status_t
+cpu_build_topology_tree(void)
+{
+	sCPUTopology.level = CPU_TOPOLOGY_LEVELS;
+
+	int32 maxID[CPU_TOPOLOGY_LEVELS];
+	memset(&maxID, 0, sizeof(maxID));
+
+	const int32 kCPUCount = smp_get_num_cpus();
+	for (int32 i = 0; i < kCPUCount; i++) {
+		for (int32 j = 0; j < CPU_TOPOLOGY_LEVELS; j++)
+			maxID[j] = max_c(maxID[j], gCPU[i].topology_id[j]);
+	}
+
+	for (int32 j = 0; j < CPU_TOPOLOGY_LEVELS; j++)
+		maxID[j]++;
+
+	sCPUTopology.children_count = maxID[CPU_TOPOLOGY_LEVELS - 1];
+	sCPUTopology.children
+		= new(std::nothrow) cpu_topology_node*[maxID[CPU_TOPOLOGY_LEVELS - 1]];
+	if (sCPUTopology.children == NULL)
+		return B_NO_MEMORY;
+	memset(sCPUTopology.children, 0,
+		maxID[CPU_TOPOLOGY_LEVELS - 1] * sizeof(cpu_topology_node*));
+
+	for (int32 i = 0; i < kCPUCount; i++) {
+		cpu_topology_node* node = &sCPUTopology;
+		for (int32 j = CPU_TOPOLOGY_LEVELS - 1; j >= 0; j--) {
+			int32 id = gCPU[i].topology_id[j];
+			if (node->children[id] == NULL) {
+				status_t result = cpu_create_topology_node(node, maxID, id);
+				if (result != B_OK)
+					return result;
+			}
+
+			node = node->children[id];
+		}
+
+		ASSERT(node->level == CPU_TOPOLOGY_SMT);
+		node->id = i;
+	}
+
+	int32 lastID[CPU_TOPOLOGY_LEVELS];
+	memset(&lastID, 0, sizeof(lastID));
+	cpu_rebuild_topology_tree(&sCPUTopology, lastID);
+}
+
+
+cpu_topology_node*
+get_cpu_topology(void)
+{
+	return &sCPUTopology;
+}
+
+
 //	#pragma mark -
 
 
diff --git a/src/system/kernel/main.cpp b/src/system/kernel/main.cpp
index 938b3a61a6..9dedf47db3 100644
--- a/src/system/kernel/main.cpp
+++ b/src/system/kernel/main.cpp
@@ -165,6 +165,7 @@ _start(kernel_args *bootKernelArgs, int currentCPU)
 
 		TRACE("init SMP\n");
 		smp_init(&sKernelArgs);
+		cpu_build_topology_tree();
 		TRACE("init timer\n");
 		timer_init(&sKernelArgs);
 		TRACE("init real time clock\n");