From 780dd2b310facce54188b190d683f21123b5148e Mon Sep 17 00:00:00 2001 From: Javier Almansa Sobrino Date: Tue, 25 Aug 2020 16:16:29 +0100 Subject: [PATCH] Add support to export a /cpus node to the device tree. This patch creates and populates the /cpus node in a device tree based on the existing topology. It uses the minimum required nodes and properties to satisfy the binding as specified in https://www.kernel.org/doc/Documentation/devicetree/bindings/arm/cpus.txt Signed-off-by: Javier Almansa Sobrino Change-Id: I03bf4e9a6427da0a3b8ed013f93d7bc43b5c4df0 --- common/fdt_fixup.c | 171 ++++++++++++++++++++++++++++++++++++- include/common/fdt_fixup.h | 4 +- 2 files changed, 172 insertions(+), 3 deletions(-) diff --git a/common/fdt_fixup.c b/common/fdt_fixup.c index d518eb2a4..980e60d45 100644 --- a/common/fdt_fixup.c +++ b/common/fdt_fixup.c @@ -1,5 +1,5 @@ /* - * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved. + * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved. * * SPDX-License-Identifier: BSD-3-Clause */ @@ -14,15 +14,20 @@ * that. */ +#include +#include #include #include +#include #include +#include +#include #include #include +#include -#include static int append_psci_compatible(void *fdt, int offs, const char *str) { @@ -210,3 +215,165 @@ int fdt_add_reserved_memory(void *dtb, const char *node_name, return 0; } + +/******************************************************************************* + * fdt_add_cpu() Add a new CPU node to the DT + * @dtb: Pointer to the device tree blob in memory + * @parent: Offset of the parent node + * @mpidr: MPIDR for the current CPU + * + * Create and add a new cpu node to a DTB. + * + * Return the offset of the new node or a negative value in case of error + ******************************************************************************/ + +static int fdt_add_cpu(void *dtb, int parent, u_register_t mpidr) +{ + int cpu_offs; + int err; + char snode_name[15]; + uint64_t reg_prop; + + reg_prop = mpidr & MPID_MASK & ~MPIDR_MT_MASK; + + snprintf(snode_name, sizeof(snode_name), "cpu@%x", + (unsigned int)reg_prop); + + cpu_offs = fdt_add_subnode(dtb, parent, snode_name); + if (cpu_offs < 0) { + ERROR ("FDT: add subnode \"%s\" failed: %i\n", + snode_name, cpu_offs); + return cpu_offs; + } + + err = fdt_setprop_string(dtb, cpu_offs, "compatible", "arm,armv8"); + if (err < 0) { + ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n", + "compatible", cpu_offs); + return err; + } + + err = fdt_setprop_u64(dtb, cpu_offs, "reg", reg_prop); + if (err < 0) { + ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n", + "reg", cpu_offs); + return err; + } + + err = fdt_setprop_string(dtb, cpu_offs, "device_type", "cpu"); + if (err < 0) { + ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n", + "device_type", cpu_offs); + return err; + } + + err = fdt_setprop_string(dtb, cpu_offs, "enable-method", "psci"); + if (err < 0) { + ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n", + "enable-method", cpu_offs); + return err; + } + + return cpu_offs; +} + +/****************************************************************************** + * fdt_add_cpus_node() - Add the cpus node to the DTB + * @dtb: pointer to the device tree blob in memory + * @afflv0: Maximum number of threads per core (affinity level 0). + * @afflv1: Maximum number of CPUs per cluster (affinity level 1). + * @afflv2: Maximum number of clusters (affinity level 2). + * + * Iterate over all the possible MPIDs given the maximum affinity levels and + * add a cpus node to the DTB with all the valid CPUs on the system. + * If there is already a /cpus node, exit gracefully + * + * A system with two CPUs would generate a node equivalent or similar to: + * + * cpus { + * #address-cells = <2>; + * #size-cells = <0>; + * + * cpu0: cpu@0 { + * compatible = "arm,armv8"; + * reg = <0x0 0x0>; + * device_type = "cpu"; + * enable-method = "psci"; + * }; + * cpu1: cpu@10000 { + * compatible = "arm,armv8"; + * reg = <0x0 0x100>; + * device_type = "cpu"; + * enable-method = "psci"; + * }; + * }; + * + * Full documentation about the CPU bindings can be found at: + * https://www.kernel.org/doc/Documentation/devicetree/bindings/arm/cpus.txt + * + * Return the offset of the node or a negative value on error. + ******************************************************************************/ + +int fdt_add_cpus_node(void *dtb, unsigned int afflv0, + unsigned int afflv1, unsigned int afflv2) +{ + int offs; + int err; + unsigned int i, j, k; + u_register_t mpidr; + int cpuid; + + if (fdt_path_offset(dtb, "/cpus") >= 0) { + return -EEXIST; + } + + offs = fdt_add_subnode(dtb, 0, "cpus"); + if (offs < 0) { + ERROR ("FDT: add subnode \"cpus\" node to parent node failed"); + return offs; + } + + err = fdt_setprop_u32(dtb, offs, "#address-cells", 2); + if (err < 0) { + ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n", + "#address-cells", offs); + return err; + } + + err = fdt_setprop_u32(dtb, offs, "#size-cells", 0); + if (err < 0) { + ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n", + "#size-cells", offs); + return err; + } + + /* + * Populate the node with the CPUs. + * As libfdt prepends subnodes within a node, reverse the index count + * so the CPU nodes would be better ordered. + */ + for (i = afflv2; i > 0U; i--) { + for (j = afflv1; j > 0U; j--) { + for (k = afflv0; k > 0U; k--) { + mpidr = ((i - 1) << MPIDR_AFF2_SHIFT) | + ((j - 1) << MPIDR_AFF1_SHIFT) | + ((k - 1) << MPIDR_AFF0_SHIFT) | + (read_mpidr_el1() & MPIDR_MT_MASK); + + cpuid = plat_core_pos_by_mpidr(mpidr); + if (cpuid >= 0) { + /* Valid MPID found */ + err = fdt_add_cpu(dtb, offs, mpidr); + if (err < 0) { + ERROR ("FDT: %s 0x%08x\n", + "error adding CPU", + (uint32_t)mpidr); + return err; + } + } + } + } + } + + return offs; +} diff --git a/include/common/fdt_fixup.h b/include/common/fdt_fixup.h index 0248de9cf..29d8b3aa0 100644 --- a/include/common/fdt_fixup.h +++ b/include/common/fdt_fixup.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved. + * Copyright (c) 2019-2020, ARM Limited and Contributors. All rights reserved. * * SPDX-License-Identifier: BSD-3-Clause */ @@ -11,5 +11,7 @@ int dt_add_psci_node(void *fdt); int dt_add_psci_cpu_enable_methods(void *fdt); int fdt_add_reserved_memory(void *dtb, const char *node_name, uintptr_t base, size_t size); +int fdt_add_cpus_node(void *dtb, unsigned int afflv0, + unsigned int afflv1, unsigned int afflv2); #endif /* FDT_FIXUP_H */