Signed-off-by: Sascha Hauer <s.ha...@pengutronix.de> --- common/oftree.c | 8 ++- drivers/of/base.c | 180 ++++++++++++++++++++++++++++++++++++++++++++--------- include/of.h | 4 +- 3 files changed, 160 insertions(+), 32 deletions(-)
diff --git a/common/oftree.c b/common/oftree.c index 0df5209..841d2c4 100644 --- a/common/oftree.c +++ b/common/oftree.c @@ -329,7 +329,13 @@ struct fdt_header *of_get_fixed_tree(struct fdt_header *fdt) int size, align; if (!fdt) { - fdt = internalfdt = of_flatten_dtb(); + struct device_node *root_node; + + root_node = of_get_root_node(); + if (!root_node) + return NULL; + + fdt = internalfdt = of_flatten_dtb(root_node); if (!fdt) return NULL; } diff --git a/drivers/of/base.c b/drivers/of/base.c index d6ca949..cd463e9 100644 --- a/drivers/of/base.c +++ b/drivers/of/base.c @@ -1154,20 +1154,108 @@ err: return ERR_PTR(ret); } -static int __of_flatten_dtb(void *fdt, struct device_node *node) +struct fdt { + void *dt; + uint32_t dt_nextofs; + uint32_t dt_size; + char *strings; + uint32_t str_nextofs; + uint32_t str_size; +}; + +static inline uint32_t dt_next_ofs(uint32_t curofs, uint32_t len) +{ + return ALIGN(curofs + len, 4); +} + +static int lstrcpy(char *dest, const char *src) +{ + int len = 0; + int maxlen = 1023; + + while (*src) { + *dest++ = *src++; + len++; + if (!maxlen) + return -ENOSPC; + maxlen--; + } + + return len; +} + +static int fdt_ensure_space(struct fdt *fdt, int dtsize) +{ + /* + * We assume strings and names have a maximum length of 1024 + * whereas properties can be longer. We allocate new memory + * if we have less than 1024 bytes (+ the property size left. + */ + if (fdt->str_size - fdt->str_nextofs < 1024) { + fdt->strings = realloc(fdt->strings, fdt->str_size * 2); + if (!fdt->strings) + return -ENOMEM; + fdt->str_size *= 2; + } + + if (fdt->dt_size - fdt->dt_nextofs < 1024 + dtsize) { + fdt->dt = realloc(fdt->dt, fdt->dt_size * 2); + if (!fdt->dt) + return -ENOMEM; + fdt->dt_size *= 2; + } + + return 0; +} + +static inline int dt_add_string(struct fdt *fdt, const char *str) +{ + uint32_t ret; + int len; + + if (fdt_ensure_space(fdt, 0) < 0) + return -ENOMEM; + + len = lstrcpy(fdt->strings + fdt->str_nextofs, str); + if (len < 0) + return -ENOSPC; + + ret = fdt->str_nextofs; + + fdt->str_nextofs += len + 1; + + return ret; +} + +static int __of_flatten_dtb(struct fdt *fdt, struct device_node *node) { struct property *p; struct device_node *n; int ret; + unsigned int len; + struct fdt_node_header *nh = fdt->dt + fdt->dt_nextofs; - ret = fdt_begin_node(fdt, node->name); - if (ret) - return ret; + if (fdt_ensure_space(fdt, 0) < 0) + return -ENOMEM; + + nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE); + len = lstrcpy(nh->name, node->name); + fdt->dt_nextofs = dt_next_ofs(fdt->dt_nextofs, 4 + len + 1); list_for_each_entry(p, &node->properties, list) { - ret = fdt_property(fdt, p->name, p->value, p->length); - if (ret) - return ret; + struct fdt_property *fp; + + if (fdt_ensure_space(fdt, p->length) < 0) + return -ENOMEM; + + fp = fdt->dt + fdt->dt_nextofs; + + fp->tag = cpu_to_fdt32(FDT_PROP); + fp->len = cpu_to_fdt32(p->length); + fp->nameoff = cpu_to_fdt32(dt_add_string(fdt, p->name)); + memcpy(fp->data, p->value, p->length); + fdt->dt_nextofs = dt_next_ofs(fdt->dt_nextofs, + sizeof(struct fdt_property) + p->length); } list_for_each_entry(n, &node->children, parent_list) { @@ -1176,45 +1264,79 @@ static int __of_flatten_dtb(void *fdt, struct device_node *node) return ret; } - ret = fdt_end_node(fdt); + nh = fdt->dt + fdt->dt_nextofs; + nh->tag = cpu_to_fdt32(FDT_END_NODE); + fdt->dt_nextofs = dt_next_ofs(fdt->dt_nextofs, + sizeof(struct fdt_node_header)); - return ret; -} + if (fdt_ensure_space(fdt, 0) < 0) + return -ENOMEM; -#define DTB_SIZE SZ_128K + return 0; +} -void *of_flatten_dtb(void) +/** + * of_flatten_dtb - flatten a barebox internal devicetree to a dtb + * @node - the root node of the tree to be unflattened + */ +void *of_flatten_dtb(struct device_node *node) { - void *fdt; int ret; + struct fdt_header header = {}; + struct fdt fdt = {}; + uint32_t ofs; + struct fdt_node_header *nh; - if (!root_node) - return NULL; + header.magic = cpu_to_fdt32(FDT_MAGIC); + header.version = cpu_to_fdt32(0x11); + header.last_comp_version = cpu_to_fdt32(0x10); - fdt = malloc(DTB_SIZE); - if (!fdt) - return NULL; + fdt.dt = xzalloc(SZ_64K); + fdt.dt_size = SZ_64K; - memset(fdt, 0, DTB_SIZE); + fdt.strings = xzalloc(SZ_64K); + fdt.str_size = SZ_64K; - ret = fdt_create(fdt, DTB_SIZE); - if (ret) - goto out_free; + ofs = sizeof(struct fdt_header); - ret = fdt_finish_reservemap(fdt); - if (ret) - goto out_free; + header.off_mem_rsvmap = cpu_to_fdt32(ofs); + ofs += sizeof(struct fdt_reserve_entry); + + fdt.dt_nextofs = ofs; - ret = __of_flatten_dtb(fdt, root_node); + ret = __of_flatten_dtb(&fdt, node); if (ret) goto out_free; + nh = fdt.dt + fdt.dt_nextofs; + nh->tag = cpu_to_fdt32(FDT_END); + fdt.dt_nextofs = dt_next_ofs(fdt.dt_nextofs, sizeof(struct fdt_node_header)); + + header.size_dt_strings = cpu_to_fdt32(fdt.str_nextofs); + header.size_dt_struct = cpu_to_fdt32(fdt.dt_nextofs); + + header.off_dt_struct = cpu_to_fdt32(ofs); + + header.off_dt_strings = cpu_to_fdt32(fdt.dt_nextofs); + + if (fdt.dt_size - fdt.dt_nextofs < fdt.str_nextofs) { + fdt.dt = realloc(fdt.dt, fdt.dt_nextofs + fdt.str_nextofs); + if (!fdt.dt) + goto out_free; + } + + memcpy(fdt.dt + fdt.dt_nextofs, fdt.strings, fdt.str_nextofs); + + header.totalsize = cpu_to_fdt32(fdt.dt_nextofs + fdt.str_nextofs); + + memcpy(fdt.dt, &header, sizeof(header)); - fdt_finish(fdt); + free(fdt.strings); - return fdt; + return fdt.dt; out_free: - free(fdt); + free(fdt.strings); + free(fdt.dt); return NULL; } diff --git a/include/of.h b/include/of.h index f2fd84b..e7d74d8 100644 --- a/include/of.h +++ b/include/of.h @@ -140,7 +140,7 @@ int of_set_root_node(struct device_node *); int of_alias_get_id(struct device_node *np, const char *stem); int of_device_is_stdout_path(struct device_d *dev); const char *of_get_model(void); -void *of_flatten_dtb(void); +void *of_flatten_dtb(struct device_node *node); #else static inline int of_parse_partitions(const char *cdevname, struct device_node *node) @@ -168,7 +168,7 @@ static inline const char *of_get_model(void) return NULL; } -static inline void *of_flatten_dtb(void) +static inline void *of_flatten_dtb(struct device_node *node) { return NULL; } -- 1.7.10.4 _______________________________________________ barebox mailing list barebox@lists.infradead.org http://lists.infradead.org/mailman/listinfo/barebox