Extract the logic needed to add a single cell described by a device tree node from nvmem_add_cells_from_of, and call this new function for each child node.
Signed-off-by: Michael Auchter <[email protected]> --- drivers/nvmem/core.c | 84 +++++++++++++++++++++++++------------------- 1 file changed, 48 insertions(+), 36 deletions(-) diff --git a/drivers/nvmem/core.c b/drivers/nvmem/core.c index 6cd3edb2eaf6..91979529cb07 100644 --- a/drivers/nvmem/core.c +++ b/drivers/nvmem/core.c @@ -518,54 +518,66 @@ nvmem_find_cell_by_name(struct nvmem_device *nvmem, const char *cell_id) return cell; } -static int nvmem_add_cells_from_of(struct nvmem_device *nvmem) +static int nvmem_add_cell_from_of(struct nvmem_device *nvmem, + struct device_node *child) { - struct device_node *parent, *child; struct device *dev = &nvmem->dev; struct nvmem_cell *cell; const __be32 *addr; int len; - parent = dev->of_node; + addr = of_get_property(child, "reg", &len); + if (!addr || (len < 2 * sizeof(u32))) { + dev_err(dev, "nvmem: invalid reg on %pOF\n", child); + return -EINVAL; + } - for_each_child_of_node(parent, child) { - addr = of_get_property(child, "reg", &len); - if (!addr || (len < 2 * sizeof(u32))) { - dev_err(dev, "nvmem: invalid reg on %pOF\n", child); - return -EINVAL; - } + cell = kzalloc(sizeof(*cell), GFP_KERNEL); + if (!cell) + return -ENOMEM; - cell = kzalloc(sizeof(*cell), GFP_KERNEL); - if (!cell) - return -ENOMEM; + cell->nvmem = nvmem; + cell->np = of_node_get(child); + cell->offset = be32_to_cpup(addr++); + cell->bytes = be32_to_cpup(addr); + cell->name = kasprintf(GFP_KERNEL, "%pOFn", child); + + addr = of_get_property(child, "bits", &len); + if (addr && len == (2 * sizeof(u32))) { + cell->bit_offset = be32_to_cpup(addr++); + cell->nbits = be32_to_cpup(addr); + } - cell->nvmem = nvmem; - cell->np = of_node_get(child); - cell->offset = be32_to_cpup(addr++); - cell->bytes = be32_to_cpup(addr); - cell->name = kasprintf(GFP_KERNEL, "%pOFn", child); + if (cell->nbits) + cell->bytes = DIV_ROUND_UP( + cell->nbits + cell->bit_offset, + BITS_PER_BYTE); - addr = of_get_property(child, "bits", &len); - if (addr && len == (2 * sizeof(u32))) { - cell->bit_offset = be32_to_cpup(addr++); - cell->nbits = be32_to_cpup(addr); - } + if (!IS_ALIGNED(cell->offset, nvmem->stride)) { + dev_err(dev, "cell %s unaligned to nvmem stride %d\n", + cell->name, nvmem->stride); + /* Cells already added will be freed later. */ + kfree_const(cell->name); + kfree(cell); + return -EINVAL; + } - if (cell->nbits) - cell->bytes = DIV_ROUND_UP( - cell->nbits + cell->bit_offset, - BITS_PER_BYTE); - - if (!IS_ALIGNED(cell->offset, nvmem->stride)) { - dev_err(dev, "cell %s unaligned to nvmem stride %d\n", - cell->name, nvmem->stride); - /* Cells already added will be freed later. */ - kfree_const(cell->name); - kfree(cell); - return -EINVAL; - } + nvmem_cell_add(cell); - nvmem_cell_add(cell); + return 0; +} + +static int nvmem_add_cells_from_of(struct nvmem_device *nvmem) +{ + struct device_node *parent, *child; + int rval; + + parent = nvmem->dev.of_node; + + for_each_child_of_node(parent, child) { + rval = nvmem_add_cell_from_of(nvmem, child); + if (rval) + return rval; } return 0; -- 2.25.4
