Mayank Grover | f3b7818 | 2017-12-19 13:31:30 +0530 | [diff] [blame] | 1 | #include "libufdt.h" |
| 2 | #include "ufdt_util.h" |
| 3 | |
| 4 | |
| 5 | int node_cmp(const void *a, const void *b) { |
| 6 | const struct ufdt_node *na = *(struct ufdt_node **)a; |
| 7 | const struct ufdt_node *nb = *(struct ufdt_node **)b; |
| 8 | return dto_strcmp(name_of(na), name_of(nb)); |
| 9 | } |
| 10 | |
| 11 | bool node_name_eq(const struct ufdt_node *node, const char *name, int len) { |
| 12 | if (!node) return false; |
| 13 | if (!name) return false; |
| 14 | if (dto_strncmp(name_of(node), name, len) != 0) return false; |
| 15 | if (name_of(node)[len] != '\0') return false; |
| 16 | return true; |
| 17 | } |
| 18 | |
| 19 | /* |
| 20 | * ufdt_node methods. |
| 21 | */ |
| 22 | |
| 23 | struct ufdt_node *ufdt_node_construct(void *fdtp, fdt32_t *fdt_tag_ptr) { |
| 24 | uint32_t tag = fdt32_to_cpu(*fdt_tag_ptr); |
| 25 | if (tag == FDT_PROP) { |
| 26 | struct fdt_prop_ufdt_node *res = dto_malloc(sizeof(struct fdt_prop_ufdt_node)); |
| 27 | if (res == NULL) return NULL; |
| 28 | res->parent.fdt_tag_ptr = fdt_tag_ptr; |
| 29 | res->parent.sibling = NULL; |
| 30 | res->name = get_name(fdtp, (struct ufdt_node *)res); |
| 31 | return (struct ufdt_node *)res; |
| 32 | } else { |
| 33 | struct fdt_node_ufdt_node *res = dto_malloc(sizeof(struct fdt_node_ufdt_node)); |
| 34 | if (res == NULL) return NULL; |
| 35 | res->parent.fdt_tag_ptr = fdt_tag_ptr; |
| 36 | res->parent.sibling = NULL; |
| 37 | res->child = NULL; |
| 38 | res->last_child_p = &res->child; |
| 39 | return (struct ufdt_node *)res; |
| 40 | } |
| 41 | } |
| 42 | |
| 43 | void ufdt_node_destruct(struct ufdt_node *node) { |
| 44 | if (node == NULL) return; |
| 45 | |
| 46 | if (tag_of(node) == FDT_BEGIN_NODE) { |
| 47 | ufdt_node_destruct(((struct fdt_node_ufdt_node *)node)->child); |
| 48 | } |
| 49 | |
| 50 | ufdt_node_destruct(node->sibling); |
| 51 | dto_free(node); |
| 52 | |
| 53 | return; |
| 54 | } |
| 55 | |
| 56 | int ufdt_node_add_child(struct ufdt_node *parent, struct ufdt_node *child) { |
| 57 | if (!parent || !child) return -1; |
| 58 | if (tag_of(parent) != FDT_BEGIN_NODE) return -1; |
| 59 | |
| 60 | int err = 0; |
| 61 | uint32_t child_tag = tag_of(child); |
| 62 | |
| 63 | switch (child_tag) { |
| 64 | case FDT_PROP: |
| 65 | case FDT_BEGIN_NODE: |
| 66 | // Append the child node to the last child of parant node |
| 67 | *((struct fdt_node_ufdt_node *)parent)->last_child_p = child; |
| 68 | ((struct fdt_node_ufdt_node *)parent)->last_child_p = &child->sibling; |
| 69 | break; |
| 70 | |
| 71 | default: |
| 72 | err = -1; |
| 73 | dto_error("invalid children tag type\n"); |
| 74 | } |
| 75 | |
| 76 | return err; |
| 77 | } |
| 78 | |
| 79 | /* |
| 80 | * BEGIN of FDT_PROP related methods. |
| 81 | */ |
| 82 | |
| 83 | struct ufdt_node *ufdt_node_get_subnode_by_name_len(const struct ufdt_node *node, |
| 84 | const char *name, int len) { |
| 85 | struct ufdt_node **it = NULL; |
| 86 | for_each_node(it, node) { |
| 87 | if (node_name_eq(*it, name, len)) return *it; |
| 88 | } |
| 89 | return NULL; |
| 90 | } |
| 91 | |
| 92 | struct ufdt_node *ufdt_node_get_subnode_by_name(const struct ufdt_node *node, |
| 93 | const char *name) { |
| 94 | return ufdt_node_get_subnode_by_name_len(node, name, strlen(name)); |
| 95 | } |
| 96 | |
| 97 | struct ufdt_node *ufdt_node_get_property_by_name_len( |
| 98 | const struct ufdt_node *node, const char *name, int len) { |
| 99 | if (!node) return NULL; |
| 100 | |
| 101 | struct ufdt_node **it = NULL; |
| 102 | for_each_prop(it, node) { |
| 103 | if (node_name_eq(*it, name, len)) return *it; |
| 104 | } |
| 105 | return NULL; |
| 106 | } |
| 107 | |
| 108 | struct ufdt_node *ufdt_node_get_property_by_name(const struct ufdt_node *node, |
| 109 | const char *name) { |
| 110 | return ufdt_node_get_property_by_name_len(node, name, dto_strlen(name)); |
| 111 | } |
| 112 | |
| 113 | char *ufdt_node_get_fdt_prop_data(const struct ufdt_node *node, int *out_len) { |
| 114 | if (!node || tag_of(node) != FDT_PROP) { |
| 115 | return NULL; |
| 116 | } |
| 117 | const struct fdt_property *prop = (struct fdt_property *)node->fdt_tag_ptr; |
| 118 | if (out_len != NULL) { |
| 119 | *out_len = fdt32_to_cpu(prop->len); |
| 120 | } |
| 121 | return (char *)prop->data; |
| 122 | } |
| 123 | |
| 124 | char *ufdt_node_get_fdt_prop_data_by_name_len(const struct ufdt_node *node, |
| 125 | const char *name, int len, |
| 126 | int *out_len) { |
| 127 | return ufdt_node_get_fdt_prop_data( |
| 128 | ufdt_node_get_property_by_name_len(node, name, len), out_len); |
| 129 | } |
| 130 | |
| 131 | char *ufdt_node_get_fdt_prop_data_by_name(const struct ufdt_node *node, |
| 132 | const char *name, int *out_len) { |
| 133 | return ufdt_node_get_fdt_prop_data(ufdt_node_get_property_by_name(node, name), |
| 134 | out_len); |
| 135 | } |
| 136 | |
| 137 | /* |
| 138 | * END of FDT_PROP related methods. |
| 139 | */ |
| 140 | |
| 141 | /* |
| 142 | * BEGIN of searching-in-ufdt_node methods. |
| 143 | */ |
| 144 | |
| 145 | uint32_t ufdt_node_get_phandle(const struct ufdt_node *node) { |
| 146 | if (!node || tag_of(node) != FDT_BEGIN_NODE) { |
| 147 | return 0; |
| 148 | } |
| 149 | int len = 0; |
| 150 | void *ptr = ufdt_node_get_fdt_prop_data_by_name(node, "phandle", &len); |
| 151 | if (!ptr || len != sizeof(fdt32_t)) { |
| 152 | ptr = ufdt_node_get_fdt_prop_data_by_name(node, "linux,phandle", &len); |
| 153 | if (!ptr || len != sizeof(fdt32_t)) { |
| 154 | return 0; |
| 155 | } |
| 156 | } |
| 157 | return fdt32_to_cpu(*((fdt32_t *)ptr)); |
| 158 | } |
| 159 | |
| 160 | struct ufdt_node *ufdt_node_get_node_by_path_len(const struct ufdt_node *node, |
| 161 | const char *path, int len) { |
| 162 | const char *end = path + len; |
| 163 | |
| 164 | struct ufdt_node *cur = (struct ufdt_node *)node; |
| 165 | |
| 166 | while (path < end) { |
| 167 | while (path[0] == '/') path++; |
| 168 | if (path == end) return cur; |
| 169 | |
| 170 | const char *next_slash; |
| 171 | next_slash = dto_memchr(path, '/', end - path); |
| 172 | if (!next_slash) next_slash = end; |
| 173 | |
| 174 | struct ufdt_node *next = NULL; |
| 175 | |
| 176 | next = ufdt_node_get_subnode_by_name_len(cur, path, next_slash - path); |
| 177 | |
| 178 | cur = next; |
| 179 | path = next_slash; |
| 180 | if (!cur) return cur; |
| 181 | } |
| 182 | |
| 183 | return cur; |
| 184 | } |
| 185 | |
| 186 | struct ufdt_node *ufdt_node_get_node_by_path(const struct ufdt_node *node, |
| 187 | const char *path) { |
| 188 | return ufdt_node_get_node_by_path_len(node, path, dto_strlen(path)); |
| 189 | } |
| 190 | |
| 191 | /* |
| 192 | * END of searching-in-ufdt_node methods. |
| 193 | */ |
| 194 | |
| 195 | int output_property_to_fdt(struct ufdt_node *prop_node, void *fdtp, |
| 196 | struct ufdt_node_dict *props_dict) { |
| 197 | int err = 0; |
| 198 | int len = 0; |
| 199 | void *data = ufdt_node_get_fdt_prop_data(prop_node, &len); |
| 200 | int nameoff = 0; |
| 201 | struct ufdt_node *same_name_prop = |
| 202 | ufdt_node_dict_find_node(props_dict, name_of(prop_node)); |
| 203 | |
| 204 | /* |
| 205 | * There's already a property with same name, take its nameoff instead. |
| 206 | */ |
| 207 | if (same_name_prop != NULL) { |
| 208 | const struct fdt_property *prop = |
| 209 | (const struct fdt_property *)same_name_prop->fdt_tag_ptr; |
| 210 | nameoff = fdt32_to_cpu(prop->nameoff); |
| 211 | } |
| 212 | /* |
| 213 | * Modifies prop_node->fdt_tag_ptr to point to the node in new fdtp. |
| 214 | */ |
| 215 | err = fast_fdt_sw_property(fdtp, name_of(prop_node), data, len, &nameoff, |
| 216 | (struct fdt_property **)&(prop_node->fdt_tag_ptr)); |
| 217 | |
| 218 | if (err < 0) { |
| 219 | dto_error("Not enough space for the string space: %d\n", |
| 220 | fdt_size_dt_strings(fdtp)); |
| 221 | } |
| 222 | ufdt_node_dict_add(props_dict, prop_node); |
| 223 | return err; |
| 224 | } |
| 225 | |
| 226 | int output_ufdt_node_to_fdt(struct ufdt_node *node, void *fdtp, |
| 227 | struct ufdt_node_dict *props_dict) { |
| 228 | uint32_t tag = tag_of(node); |
| 229 | |
| 230 | if (tag == FDT_PROP) { |
| 231 | int err = output_property_to_fdt(node, fdtp, props_dict); |
| 232 | return err; |
| 233 | } |
| 234 | |
| 235 | int err = fdt_begin_node(fdtp, name_of(node)); |
| 236 | if (err < 0) return -1; |
| 237 | |
| 238 | struct ufdt_node **it; |
| 239 | for_each_prop(it, node) { |
| 240 | err = output_ufdt_node_to_fdt(*it, fdtp, props_dict); |
| 241 | if (err < 0) return -1; |
| 242 | } |
| 243 | |
| 244 | for_each_node(it, node) { |
| 245 | err = output_ufdt_node_to_fdt(*it, fdtp, props_dict); |
| 246 | if (err < 0) return -1; |
| 247 | } |
| 248 | |
| 249 | err = fdt_end_node(fdtp); |
| 250 | if (err < 0) return -1; |
| 251 | |
| 252 | return 0; |
| 253 | } |
| 254 | |
| 255 | #define TAB_SIZE 2 |
| 256 | |
| 257 | void ufdt_node_print(const struct ufdt_node *node, int depth) { |
| 258 | if (!node) return; |
| 259 | |
| 260 | int i; |
| 261 | for (i = 0; i < depth * TAB_SIZE; i++) dto_print(" "); |
| 262 | |
| 263 | uint32_t tag; |
| 264 | tag = tag_of(node); |
| 265 | |
| 266 | switch (tag) { |
| 267 | case FDT_BEGIN_NODE: |
| 268 | dto_print("NODE "); |
| 269 | break; |
| 270 | case FDT_PROP: |
| 271 | dto_print("PROP "); |
| 272 | break; |
| 273 | default: |
| 274 | dto_print("UNKNOWN "); |
| 275 | break; |
| 276 | } |
| 277 | |
| 278 | if (name_of(node)) { |
| 279 | dto_print(":%s:\n", name_of(node)); |
| 280 | } else { |
| 281 | dto_print("node name is NULL.\n"); |
| 282 | } |
| 283 | |
| 284 | if (tag_of(node) == FDT_BEGIN_NODE) { |
| 285 | struct ufdt_node **it; |
| 286 | |
| 287 | for_each_prop(it, node) ufdt_node_print(*it, depth + 1); |
| 288 | |
| 289 | for_each_node(it, node) ufdt_node_print(*it, depth + 1); |
| 290 | } |
| 291 | |
| 292 | return; |
| 293 | } |
| 294 | |
| 295 | void ufdt_node_map(struct ufdt_node *node, struct ufdt_node_closure closure) { |
| 296 | if (node == NULL) return; |
| 297 | closure.func(node, closure.env); |
| 298 | if (tag_of(node) == FDT_BEGIN_NODE) { |
| 299 | struct ufdt_node **it; |
| 300 | for_each_prop(it, node) ufdt_node_map(*it, closure); |
| 301 | for_each_node(it, node) ufdt_node_map(*it, closure); |
| 302 | } |
| 303 | return; |
| 304 | } |