From mboxrd@z Thu Jan 1 00:00:00 1970 From: Thierry Reding Date: Fri, 8 Mar 2019 21:11:38 +0100 Subject: [U-Boot] [PATCH 4/6] fdtdec: Implement fdtdec_add_reserved_memory() In-Reply-To: <20190308201140.2383-1-thierry.reding@gmail.com> References: <20190308201140.2383-1-thierry.reding@gmail.com> Message-ID: <20190308201140.2383-4-thierry.reding@gmail.com> List-Id: MIME-Version: 1.0 Content-Type: text/plain; charset="us-ascii" Content-Transfer-Encoding: 7bit To: u-boot@lists.denx.de From: Thierry Reding This function can be used to add subnodes in the /reserved-memory node. Signed-off-by: Thierry Reding --- include/fdtdec.h | 17 +++++ lib/fdtdec.c | 158 +++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 175 insertions(+) diff --git a/include/fdtdec.h b/include/fdtdec.h index 997103a87cdf..5c9108ced571 100644 --- a/include/fdtdec.h +++ b/include/fdtdec.h @@ -979,6 +979,23 @@ int fdtdec_get_max_phandle(const void *blob, uint32_t *maxp); */ int fdtdec_set_phandle(void *blob, int node, uint32_t phandle); +/** + * fdtdec_add_reserved_memory() - add or find a reserved-memory node + * + * If a reserved-memory node already exists for the given carveout, a phandle + * for that node will be returned. Otherwise a new node will be created and a + * phandle corresponding to it will be returned. + * + * @param blob FDT blob + * @param basename base name of the node to create + * @param carveout information about the carveout region + * @param phandlep return location for the phandle of the carveout region + * @return 0 on success or a negative error code on failure + */ +int fdtdec_add_reserved_memory(void *blob, const char *basename, + const struct fdt_memory *carveout, + uint32_t *phandlep); + /** * Set up the device tree ready for use */ diff --git a/lib/fdtdec.c b/lib/fdtdec.c index 9195a05d1129..a8b35c144ae0 100644 --- a/lib/fdtdec.c +++ b/lib/fdtdec.c @@ -1287,6 +1287,164 @@ int fdtdec_set_phandle(void *blob, int node, uint32_t phandle) return 0; } +static int fdtdec_init_reserved_memory(void *blob) +{ + int na, ns, node, err; + fdt32_t value; + + /* inherit #address-cells and #size-cells from the root node */ + na = fdt_address_cells(blob, 0); + ns = fdt_size_cells(blob, 0); + + node = fdt_add_subnode(blob, 0, "reserved-memory"); + if (node < 0) + return node; + + err = fdt_setprop(blob, node, "ranges", NULL, 0); + if (err < 0) + return err; + + value = cpu_to_fdt32(na); + + err = fdt_setprop(blob, node, "#address-cells", &value, sizeof(value)); + if (err < 0) + return err; + + value = cpu_to_fdt32(ns); + + err = fdt_setprop(blob, node, "#size-cells", &value, sizeof(value)); + if (err < 0) + return err; + + return node; +} + +static void fdt_addr_unpack(fdt_addr_t addr, fdt32_t *upper, fdt32_t *lower) +{ +#ifdef CONFIG_PHYS_64BIT + *upper = addr >> 32; +#else + *upper = 0; +#endif + + *lower = addr; +} + +static void fdt_size_unpack(fdt_size_t size, fdt32_t *upper, fdt32_t *lower) +{ +#ifdef CONFIG_PHYS_64BIT + *upper = size >> 32; +#else + *upper = 0; +#endif + + *lower = size; +} + +int fdtdec_add_reserved_memory(void *blob, const char *basename, + const struct fdt_memory *carveout, + uint32_t *phandlep) +{ + fdt32_t cells[4] = {}, *ptr = cells; + uint32_t upper, lower, phandle; + int parent, node, na, ns, err; + char name[64]; + + /* create an empty /reserved-memory node if one doesn't exist */ + parent = fdt_path_offset(blob, "/reserved-memory"); + if (parent < 0) { + parent = fdtdec_init_reserved_memory(blob); + if (parent < 0) + return parent; + } + + /* only 1 or 2 #address-cells and #size-cells are supported */ + na = fdt_address_cells(blob, parent); + if (na < 1 || na > 2) + return -FDT_ERR_BADNCELLS; + + ns = fdt_address_cells(blob, parent); + if (ns < 1 || ns > 2) + return -FDT_ERR_BADNCELLS; + + /* find a matching node and return the phandle to that */ + fdt_for_each_subnode(node, blob, parent) { + const char *name = fdt_get_name(blob, node, NULL); + phys_addr_t addr, size; + + addr = fdtdec_get_addr_size(blob, node, "reg", &size); + if (addr == FDT_ADDR_T_NONE) { + printf("failed to read address/size for %s\n", name); + continue; + } + + if (addr == carveout->start && (addr + size) == carveout->end) { + *phandlep = fdt_get_phandle(blob, node); + return 0; + } + } + + /* + * Unpack the start address and generate the name of the new node + * base on the basename and the unit-address. + */ + fdt_addr_unpack(carveout->start, &upper, &lower); + + if (na > 1) + snprintf(name, sizeof(name), "%s@%x,%x", basename, upper, + lower); + else { + if (upper) { + printf("address %08x:%08x exceeds addressable space\n", + upper, lower); + return -FDT_ERR_BADVALUE; + } + + snprintf(name, sizeof(name), "%s@%x", basename, lower); + } + + node = fdt_add_subnode(blob, parent, name); + if (node < 0) + return node; + + /* + * Generate a new phandle for the reserved-memory node. Look up the + * highest phandle number currently in used and use the next higher + * one. + */ + err = fdtdec_get_max_phandle(blob, &phandle); + if (err < 0) + return err; + + err = fdtdec_set_phandle(blob, node, phandle + 1); + if (err < 0) + return err; + + /* store one or two address cells */ + if (na > 1) + *ptr++ = cpu_to_fdt32(upper); + + *ptr++ = cpu_to_fdt32(lower); + + /* store one or two size cells */ + fdt_size_unpack(carveout->end - carveout->start, &upper, &lower); + + if (ns > 1) + *ptr++ = cpu_to_fdt32(upper); + + *ptr++ = cpu_to_fdt32(lower); + + err = fdt_setprop(blob, node, "reg", cells, ptr - cells); + if (err < 0) + return err; + + /* return the phandle for the new node for the caller to use */ + if (phandlep) + *phandlep = phandle + 1; + + return 0; +} + int fdtdec_setup(void) { #if CONFIG_IS_ENABLED(OF_CONTROL) -- 2.20.1