Assign correct stack to CPUs.

The CPU should get the corresponding entry from the callstacks for
easier understanding. In particular, the boot CPU must get the 0th
callstack since it has already been using that stack since boot.

Change-Id: I435f5f9580baecae4be233507b72faa4da00d78f
diff --git a/src/cpu.c b/src/cpu.c
index 8f12b7e..37de1de 100644
--- a/src/cpu.c
+++ b/src/cpu.c
@@ -44,12 +44,6 @@
 
 static uint32_t cpu_count = 1;
 
-static void cpu_init(struct cpu *c)
-{
-	/* TODO: Assumes that c is zeroed out already. */
-	sl_init(&c->lock);
-}
-
 void cpu_module_init(const cpu_id_t *cpu_ids, size_t count)
 {
 	uint32_t i;
@@ -71,15 +65,17 @@
 		cpu_id_t id = cpu_ids[i];
 
 		if (found_boot_cpu || id != boot_cpu_id) {
-			c = &cpus[--j];
+			--j;
+			c = &cpus[j];
+			c->stack_bottom = &callstacks[j][STACK_SIZE];
 		} else {
 			found_boot_cpu = true;
 			c = &cpus[0];
+			CHECK(c->stack_bottom == &callstacks[0][STACK_SIZE]);
 		}
 
-		cpu_init(c);
+		sl_init(&c->lock);
 		c->id = id;
-		c->stack_bottom = &callstacks[i][STACK_SIZE];
 	}
 
 	if (!found_boot_cpu) {