From b8672a646b0eba090cdb316f3c4a6ff19f75de03 Mon Sep 17 00:00:00 2001
From: Mihai Carabas <mihai.carabas@gmail.com>
Date: Sat, 30 Jun 2012 18:11:06 +0300
Subject: [PATCH] cpu_topology: fix bug - apicids aren't consecutive

---
 sys/kern/subr_cpu_topology.c    | 22 ++++++++++++++++++++--
 sys/platform/pc64/include/smp.h |  8 +++++++-
 2 files changed, 27 insertions(+), 3 deletions(-)

diff --git a/sys/kern/subr_cpu_topology.c b/sys/kern/subr_cpu_topology.c
index 2758927ac6..ca83d4a1a1 100644
--- a/sys/kern/subr_cpu_topology.c
+++ b/sys/kern/subr_cpu_topology.c
@@ -64,6 +64,24 @@ static struct sysctl_oid *cpu_topology_sysctl_tree;
 static char cpu_topology_members[8*MAXCPU];
 static per_cpu_sysctl_info_t pcpu_sysctl[MAXCPU];
 
+/* Get the next valid apicid starting
+ * from current apicid (curr_apicid
+ */
+static int
+get_next_valid_apicid(int curr_apicid)
+{
+	int next_apicid = curr_apicid;
+	do {
+		next_apicid++;
+	}
+	while(get_cpuid_from_apicid(next_apicid) == -1 &&
+	   next_apicid < NAPICID);
+	if (next_apicid == NAPICID) {
+		kprintf("Warning: No next valid APICID found. Returning -1\n");
+		return -1;
+	}
+	return next_apicid;
+}
 
 /* Generic topology tree. The parameters have the following meaning:
  * - children_no_per_level : the number of children on each level
@@ -89,8 +107,8 @@ build_topology_tree(int *children_no_per_level,
 
 	if (node->child_no == 0) {
 		node->child_node = NULL;
+		*apicid = get_next_valid_apicid(*apicid);
 		node->members = CPUMASK(get_cpuid_from_apicid(*apicid));
-		(*apicid)++;
 		return;
 	}
 
@@ -127,7 +145,7 @@ build_cpu_topology(void)
 	int chips_per_package = 0;
 	int children_no_per_level[LEVEL_NO];
 	uint8_t level_types[LEVEL_NO];
-	int apicid = 0;
+	int apicid = -1;
 
 	cpu_node_t *root = &cpu_topology_nodes[0];
 	cpu_node_t *last_free_node = root + 1;
diff --git a/sys/platform/pc64/include/smp.h b/sys/platform/pc64/include/smp.h
index b087a66328..3d914bc3d1 100644
--- a/sys/platform/pc64/include/smp.h
+++ b/sys/platform/pc64/include/smp.h
@@ -102,14 +102,20 @@ int get_cpuid_from_apicid(int apicid) {
 }
 
 #include <sys/systm.h>
+#define NAPICID 256
 static __inline
 void print_apic_ids(void) {
 	int i;
 	kprintf ("APIC IDs: ");
-	for (i = 0; i < ncpus; i++) {
+	for (i = 0; i < NAPICID; i++) {
 		kprintf ("%d ",CPUID_TO_APICID(i));
 	}
+	kprintf("\nCPUIDs:");
+	for (i = 0; i < NAPICID; i++) {
+		kprintf ("%d ",APICID_TO_CPUID(i));
+	}
 	kprintf("\n");
+
 }
 
 #endif /* !LOCORE */
-- 
2.41.0