Get the cpu IDs from the FDT.
These are the IDs used to identify cores in PSCI.
Change-Id: I9e88a6c69f963864591e39f0191cdd5ce824ce6d
diff --git a/src/arch/aarch64/cpu.c b/src/arch/aarch64/cpu.c
index 1f8ac31..4a8114a 100644
--- a/src/arch/aarch64/cpu.c
+++ b/src/arch/aarch64/cpu.c
@@ -33,8 +33,8 @@
__asm__ volatile("msr DAIFClr, #0xf");
}
-void arch_regs_reset(struct arch_regs *r, bool is_primary, uint64_t vmid,
- paddr_t table, uint32_t index)
+void arch_regs_reset(struct arch_regs *r, bool is_primary, uint64_t vm_id,
+ uint64_t vcpu_id, paddr_t table)
{
uintreg_t pc = r->pc;
uintreg_t arg = r->r[0];
@@ -74,8 +74,8 @@
r->lazy.hcr_el2 = hcr;
r->lazy.cptr_el2 = cptr;
r->lazy.cnthctl_el2 = cnthctl;
- r->lazy.vttbr_el2 = pa_addr(table) | (vmid << 48);
- r->lazy.vmpidr_el2 = index;
+ r->lazy.vttbr_el2 = pa_addr(table) | (vm_id << 48);
+ r->lazy.vmpidr_el2 = vcpu_id;
/* TODO: Use constant here. */
r->spsr = 5 | /* M bits, set to EL1h. */
(0xf << 6); /* DAIF bits set; disable interrupts. */
diff --git a/src/arch/aarch64/hypervisor_entry.S b/src/arch/aarch64/hypervisor_entry.S
index 468c684..9c7fa95 100644
--- a/src/arch/aarch64/hypervisor_entry.S
+++ b/src/arch/aarch64/hypervisor_entry.S
@@ -14,6 +14,8 @@
* limitations under the License.
*/
+#include "offsets.h"
+
.section .init.image_entry, "ax"
.global image_entry
image_entry:
@@ -24,4 +26,11 @@
adrp x0, cpus
add x0, x0, :lo12:cpus
+ /* Set the ID of this cpu from the affinity bits of mpidr. */
+ mrs x30, mpidr_el1
+ ubfx x29, x30, 0, 24
+ ubfx x30, x30, 32, 8
+ orr x30, x29, x30
+ str x30, [x0, CPU_ID]
+
b cpu_entry
diff --git a/src/arch/aarch64/offsets.c b/src/arch/aarch64/offsets.c
index 2cf5be8..2e5a0d4 100644
--- a/src/arch/aarch64/offsets.c
+++ b/src/arch/aarch64/offsets.c
@@ -27,6 +27,7 @@
"Offset " name " should be " #expected \
" and not " #actual)
+CHECK_OFFSET(CPU_ID, struct cpu, id);
CHECK_OFFSET(CPU_STACK_BOTTOM, struct cpu, stack_bottom);
CHECK_OFFSET(VCPU_REGS, struct vcpu, regs);
CHECK_OFFSET(VCPU_LAZY, struct vcpu, regs.lazy);
diff --git a/src/arch/aarch64/offsets.h b/src/arch/aarch64/offsets.h
index e967fe4..5a23c0b 100644
--- a/src/arch/aarch64/offsets.h
+++ b/src/arch/aarch64/offsets.h
@@ -17,6 +17,7 @@
#pragma once
/* These are checked in offset.c. */
+#define CPU_ID 0
#define CPU_STACK_BOTTOM 8
#define VCPU_REGS 32
#define VCPU_LAZY (VCPU_REGS + 264)
diff --git a/src/arch/fake/cpu.c b/src/arch/fake/cpu.c
index a00e940..8764daf 100644
--- a/src/arch/fake/cpu.c
+++ b/src/arch/fake/cpu.c
@@ -26,14 +26,14 @@
/* TODO */
}
-void arch_regs_reset(struct arch_regs *r, bool is_primary, uint64_t vmid,
- paddr_t table, uint32_t index)
+void arch_regs_reset(struct arch_regs *r, bool is_primary, uint64_t vm_id,
+ uint64_t vcpu_id, paddr_t table)
{
/* TODO */
(void)is_primary;
- (void)vmid;
+ (void)vm_id;
(void)table;
- r->vcpu_index = index;
+ r->vcpu_id = vcpu_id;
}
void arch_regs_set_pc_arg(struct arch_regs *r, ipaddr_t pc, uintreg_t arg)
diff --git a/src/arch/fake/inc/hf/arch/types.h b/src/arch/fake/inc/hf/arch/types.h
index 7304ad3..d31c005 100644
--- a/src/arch/fake/inc/hf/arch/types.h
+++ b/src/arch/fake/inc/hf/arch/types.h
@@ -37,6 +37,6 @@
/** Type to represent the register state of a VM. */
struct arch_regs {
uintreg_t r[5];
- uintreg_t vcpu_index;
+ uintreg_t vcpu_id;
bool virtual_interrupt;
};
diff --git a/src/cpu.c b/src/cpu.c
index 2c58599..536fee1 100644
--- a/src/cpu.c
+++ b/src/cpu.c
@@ -40,18 +40,52 @@
},
};
-void cpu_module_init(void)
-{
- size_t i;
+static uint32_t cpu_count = 1;
- /* Initialize all CPUs. */
- for (i = 0; i < MAX_CPUS; i++) {
- struct cpu *c = &cpus[i];
+static void cpu_init(struct cpu *c)
+{
+ /* TODO: Assumes that c is zeroed out already. */
+ sl_init(&c->lock);
+ c->irq_disable_count = 1;
+}
+
+void cpu_module_init(const uint64_t *cpu_ids, size_t count)
+{
+ uint32_t i;
+ uint32_t j;
+ uint64_t boot_cpu_id = cpus[0].id;
+ bool found_boot_cpu = false;
+
+ cpu_count = count;
+
+ /*
+ * Initialize CPUs with the IDs from the configuration passed in. The
+ * CPUs after the boot CPU are initialized in reverse order. The boot
+ * CPU is initialized when it is found or in place of the last CPU if it
+ * is not found.
+ */
+ j = cpu_count;
+ for (i = 0; i < cpu_count; ++i) {
+ struct cpu *c;
+ uint64_t id = cpu_ids[i];
+
+ if (found_boot_cpu || id != boot_cpu_id) {
+ c = &cpus[--j];
+ } else {
+ found_boot_cpu = true;
+ c = &cpus[0];
+ }
cpu_init(c);
- c->id = i; /* TODO: Initialize ID based on fdt. */
+ c->id = id;
c->stack_bottom = &callstacks[i][STACK_SIZE];
}
+
+ if (!found_boot_cpu) {
+ /* Boot CPU was initialized but with wrong ID. */
+ dlog("Boot CPU's ID not found in config.");
+ cpus[0].id = boot_cpu_id;
+ }
}
size_t cpu_index(struct cpu *c)
@@ -59,13 +93,6 @@
return c - cpus;
}
-void cpu_init(struct cpu *c)
-{
- /* TODO: Assumes that c is zeroed out already. */
- sl_init(&c->lock);
- c->irq_disable_count = 1;
-}
-
void cpu_irq_enable(struct cpu *c)
{
c->irq_disable_count--;
@@ -117,11 +144,11 @@
/**
* Searches for a CPU based on its id.
*/
-struct cpu *cpu_find(size_t id)
+struct cpu *cpu_find(uint64_t id)
{
size_t i;
- for (i = 0; i < MAX_CPUS; i++) {
+ for (i = 0; i < cpu_count; i++) {
if (cpus[i].id == id) {
return &cpus[i];
}
diff --git a/src/fdt_handler.c b/src/fdt_handler.c
index 7afb2b4..f41fa8d 100644
--- a/src/fdt_handler.c
+++ b/src/fdt_handler.c
@@ -17,6 +17,7 @@
#include "hf/fdt_handler.h"
#include "hf/boot_params.h"
+#include "hf/cpu.h"
#include "hf/dlog.h"
#include "hf/fdt.h"
#include "hf/layout.h"
@@ -125,6 +126,57 @@
return true;
}
+void fdt_find_cpus(const struct fdt_node *root, uint64_t *cpu_ids,
+ size_t *cpu_count)
+{
+ struct fdt_node n = *root;
+ const char *name;
+ uint64_t address_size;
+
+ *cpu_count = 0;
+
+ if (!fdt_find_child(&n, "cpus")) {
+ dlog("Unable to find 'cpus'\n");
+ return;
+ }
+
+ if (fdt_read_number(&n, "#address-cells", &address_size)) {
+ address_size *= sizeof(uint32_t);
+ } else {
+ address_size = sizeof(uint32_t);
+ }
+
+ if (!fdt_first_child(&n, &name)) {
+ return;
+ }
+
+ do {
+ const char *data;
+ uint32_t size;
+
+ if (!fdt_read_property(&n, "device_type", &data, &size) ||
+ size != sizeof("cpu") ||
+ memcmp(data, "cpu", sizeof("cpu")) != 0 ||
+ !fdt_read_property(&n, "reg", &data, &size)) {
+ continue;
+ }
+
+ /* Get all entries for this CPU. */
+ while (size >= address_size) {
+ if (*cpu_count >= MAX_CPUS) {
+ dlog("Found more than %d CPUs\n", MAX_CPUS);
+ return;
+ }
+
+ cpu_ids[(*cpu_count)++] =
+ convert_number(data, address_size);
+
+ size -= address_size;
+ data += address_size;
+ }
+ } while (fdt_next_sibling(&n, &name));
+}
+
void fdt_find_memory_ranges(const struct fdt_node *root, struct boot_params *p)
{
struct fdt_node n = *root;
diff --git a/src/load.c b/src/load.c
index 11e5568..30de46f 100644
--- a/src/load.c
+++ b/src/load.c
@@ -348,7 +348,7 @@
dlog("Loaded with %u vcpus, entry at 0x%x\n", cpu,
pa_addr(secondary_mem_begin));
- vm_start_vcpu(vm, 0, secondary_entry, 0);
+ vm_secondary_start_vcpu(vm, 0, secondary_entry, 0);
}
/*
diff --git a/src/main.c b/src/main.c
index 969cacf..3457f25 100644
--- a/src/main.c
+++ b/src/main.c
@@ -80,8 +80,6 @@
mpool_init(&ppool, sizeof(struct mm_page_table));
mpool_add_chunk(&ppool, ptable_buf, sizeof(ptable_buf));
- cpu_module_init();
-
if (!mm_init(&ppool)) {
panic("mm_init failed");
}
@@ -94,6 +92,8 @@
panic("unable to retrieve boot params");
}
+ cpu_module_init(params.cpu_ids, params.cpu_count);
+
for (i = 0; i < params.mem_ranges_count; ++i) {
dlog("Memory range: 0x%x - 0x%x\n",
pa_addr(params.mem_ranges[i].begin),
@@ -174,8 +174,7 @@
vcpu->cpu = c;
/* Reset the registers to give a clean start for the primary's vCPU. */
- arch_regs_reset(&vcpu->regs, true, vm->id, vm->ptable.root,
- vcpu_index(vcpu));
+ arch_regs_reset(&vcpu->regs, true, vm->id, c->id, vm->ptable.root);
return vcpu;
}
diff --git a/src/plat.c b/src/plat.c
index d7d0566..2172d6b 100644
--- a/src/plat.c
+++ b/src/plat.c
@@ -83,6 +83,8 @@
goto out_unmap_fdt;
}
+ fdt_find_cpus(&n, p->cpu_ids, &p->cpu_count);
+
p->mem_ranges_count = 0;
fdt_find_memory_ranges(&n, p);
diff --git a/src/vm.c b/src/vm.c
index 15e7521..b68c219 100644
--- a/src/vm.c
+++ b/src/vm.c
@@ -103,14 +103,27 @@
locked->vm = NULL;
}
-/* TODO: Shall we use index or id here? */
-void vm_start_vcpu(struct vm *vm, size_t index, ipaddr_t entry, uintreg_t arg)
+/**
+ * Starts a vCPU of a secondary VM.
+ *
+ * TODO: Shall we use index or id here?
+ */
+void vm_secondary_start_vcpu(struct vm *vm, size_t index, ipaddr_t entry,
+ uintreg_t arg)
{
struct vcpu *vcpu = &vm->vcpus[index];
- if (index < vm->vcpu_count) {
- vcpu_on(vcpu, entry, arg);
- arch_regs_reset(&vcpu->regs, vm->id == HF_PRIMARY_VM_ID, vm->id,
- vm->ptable.root, vcpu_index(vcpu));
+ if (index >= vm->vcpu_count) {
+ return;
}
+
+ /*
+ * Set vCPU registers to a clean state ready for boot. As this is a
+ * secondary which can migrate between pCPUs, the ID of the vCPU is
+ * defined as the index and does not match the ID of the pCPU it is
+ * running on.
+ */
+ arch_regs_reset(&vcpu->regs, vm->id == HF_PRIMARY_VM_ID, vm->id, index,
+ vm->ptable.root);
+ vcpu_on(vcpu, entry, arg);
}