From: Ruslan Ruslichenko <[email protected]> The patch adds few utility functions for parsing FDT nodes. The helpers are required for upcoming Hardware device tree feature.
Signed-off-by: Ruslan Ruslichenko <[email protected]> --- include/system/device_tree.h | 30 +++++ system/device_tree.c | 221 +++++++++++++++++++++++++++++++++++ 2 files changed, 251 insertions(+) diff --git a/include/system/device_tree.h b/include/system/device_tree.h index 5667ff9538..9d6b570bdb 100644 --- a/include/system/device_tree.h +++ b/include/system/device_tree.h @@ -116,6 +116,11 @@ const void *qemu_fdt_getprop(void *fdt, const char *node_path, uint32_t qemu_fdt_getprop_cell(void *fdt, const char *node_path, const char *property, int cell_id, Error **errp); + +const void *qemu_fdt_getprop_inherited(void *fdt, const char *node_path, + const char *property, int *lenp, Error **errp); +uint32_t qemu_fdt_getprop_cell_inherited(void *fdt, const char *node_path, + const char *property, int cell_id, Error **errp); uint32_t qemu_fdt_get_phandle(void *fdt, const char *path); uint32_t qemu_fdt_alloc_phandle(void *fdt); int qemu_fdt_nop_node(void *fdt, const char *node_path); @@ -191,6 +196,31 @@ int qemu_fdt_setprop_sized_cells_from_array(void *fdt, qdt_tmp); \ }) +typedef struct QEMUDevtreeProp { + const char *name; + int len; + const void *value; +} QEMUDevtreeProp; + +/* node queries */ + +int qemu_devtree_get_num_children(void *fdt, const char *node_path); +int qemu_devtree_get_children(void *fdt, const char *node_path, + int max_paths, char **returned_paths); +int qemu_devtree_num_props(void *fdt, const char *node_path); +QEMUDevtreeProp *qemu_devtree_get_props(void *fdt, const char *node_path); + +/* node getters */ + +int qemu_devtree_get_node_by_phandle(void *fdt, char *node_path, int phandle); +int qemu_devtree_getparent(void *fdt, char *node_path, + const char *current); + +/* misc */ + +int devtree_get_num_nodes(void *fdt); + +#define DT_PATH_LENGTH 1024 /** * qemu_fdt_randomize_seeds: diff --git a/system/device_tree.c b/system/device_tree.c index d2db7bd355..fc34c660db 100644 --- a/system/device_tree.c +++ b/system/device_tree.c @@ -464,6 +464,103 @@ uint32_t qemu_fdt_getprop_cell(void *fdt, const char *node_path, return be32_to_cpu(p[cell_id]); } +const void *qemu_fdt_getprop_inherited(void *fdt, const char *node_path, + const char *property, int *lenp, Error **errp) +{ + const void *found_val = NULL; + int found_len = 0; + int curr_offset = 0; + int temp_len; + char **tokens; + char **iter; + + found_val = fdt_getprop(fdt, 0, property, &found_len); + + tokens = g_strsplit(node_path + 1, "/", -1); + + for (iter = tokens; *iter != NULL; iter++) { + if (**iter == '\0') { + continue; + } + + curr_offset = fdt_subnode_offset(fdt, curr_offset, *iter); + if (curr_offset < 0) { + error_setg(errp, "%s: Path '%s' not found", + __func__, *iter); + g_strfreev(tokens); + return NULL; + } + + const void *val = fdt_getprop(fdt, curr_offset, property, &temp_len); + if (val) { + found_val = val; + found_len = temp_len; + } + } + g_strfreev(tokens); + + if (!found_val) { + error_setg(errp, "%s: Property '%s' not found", + __func__, property); + return NULL; + } + + if (lenp) { + *lenp = found_len; + } + + return found_val; +} + +uint32_t qemu_fdt_getprop_cell_inherited(void *fdt, const char *node_path, + const char *property, int cell_id, Error **errp) +{ + int len; + const uint32_t *p; + + p = qemu_fdt_getprop_inherited(fdt, node_path, property, &len, errp); + if (!p) { + return 0; + } + if (len < (cell_id + 1) * 4) { + error_setg(errp, + "%s: %s/%s is too short, need %d bytes for cell ind %d", + __func__, node_path, property, (cell_id + 1) * 4, cell_id); + return 0; + } + return be32_to_cpu(p[cell_id]); +} + +int qemu_devtree_getparent(void *fdt, char *node_path, const char *current) +{ + const char *sep; + int len; + + if (!current || !node_path) { + return -FDT_ERR_NOTFOUND; + } + + if (current[0] == '/' && current[1] == 0) { + return -FDT_ERR_NOTFOUND; + } + + sep = strrchr(current, '/'); + if (!sep) { + return -FDT_ERR_BADPATH; + } + + if (sep == current) { + len = 1; + } else { + len = sep - current; + } + + memcpy(node_path, current, len); + node_path[len] = '\0'; + + return 0; +} + uint32_t qemu_fdt_get_phandle(void *fdt, const char *path) { uint32_t r; @@ -631,6 +728,130 @@ out: return ret; } +int qemu_devtree_num_props(void *fdt, const char *node_path) +{ + int offset = fdt_path_offset(fdt, node_path); + int ret = 0; + + for (offset = fdt_first_property_offset(fdt, offset); + offset != -FDT_ERR_NOTFOUND; + offset = fdt_next_property_offset(fdt, offset)) { + ret++; + } + return ret; +} + +QEMUDevtreeProp *qemu_devtree_get_props(void *fdt, const char *node_path) +{ + QEMUDevtreeProp *ret = g_new0(QEMUDevtreeProp, + qemu_devtree_num_props(fdt, node_path) + 1); + int offset = fdt_path_offset(fdt, node_path); + int i = 0; + + for (offset = fdt_first_property_offset(fdt, offset); + offset != -FDT_ERR_NOTFOUND; + offset = fdt_next_property_offset(fdt, offset)) { + const char *propname; + const void *val = fdt_getprop_by_offset(fdt, offset, &propname, + &ret[i].len); + + ret[i].name = propname; + ret[i].value = val; + i++; + } + return ret; +} + +int qemu_devtree_get_children(void *fdt, const char *node_path, + int max_paths, char **returned_paths) { + int count = 0; + int subnode; + const char *name; + int offset = fdt_path_offset(fdt, node_path); + + if (offset < 0) { + return offset; + } + + bool is_root = (strcmp(node_path, "/") == 0); + + fdt_for_each_subnode(subnode, fdt, offset) { + if (count >= max_paths) { + break; + } + name = fdt_get_name(fdt, subnode, NULL); + if (returned_paths) { + returned_paths[count] = g_strdup_printf("%s/%s", + is_root ? "" : node_path, name); + } + + ++count; + } + + return count; +} + +int qemu_devtree_get_num_children(void *fdt, const char *node_path) +{ + int count = 0; + int subnode; + int offset = fdt_path_offset(fdt, node_path); + + if (offset < 0) { + return offset; + } + + fdt_for_each_subnode(subnode, fdt, offset) { + ++count; + } + + return count; +} + +int qemu_devtree_get_node_by_phandle(void *fdt, char *node_path, int phandle) +{ + int offset = 0, cur_depth = 0; + int path_lens[64] = { 0 }; + + for (offset = 0; offset >= 0; offset = fdt_next_node(fdt, offset, + &cur_depth)) { + if (cur_depth > 64) { + break; + } + const char *name = fdt_get_name(fdt, offset, NULL); + + int parent_len = (cur_depth > 0) ? path_lens[cur_depth - 1] : 0; + int len = snprintf(node_path + parent_len, + DT_PATH_LENGTH - parent_len, + "%s%s", + (parent_len > 1) ? "/" : "", + (cur_depth == 0) ? "/" : name); + + path_lens[cur_depth] = parent_len + len; + + if (fdt_get_phandle(fdt, offset) == phandle) { + return 0; + } + } + + return -FDT_ERR_NOTFOUND; +} + +int devtree_get_num_nodes(void *fdt) +{ + int num_nodes = 0; + int depth = 0, offset = 0; + + for (;;) { + offset = fdt_next_node(fdt, offset, &depth); + num_nodes++; + if (offset <= 0 || depth <= 0) { + break; + } + } + return num_nodes; +} + void qmp_dumpdtb(const char *filename, Error **errp) { ERRP_GUARD(); -- 2.43.0
