Mayank Grover | f3b7818 | 2017-12-19 13:31:30 +0530 | [diff] [blame] | 1 | #include "libufdt.h" |
| 2 | |
| 3 | #include "fdt_internal.h" |
| 4 | #include "ufdt_util.h" |
| 5 | |
| 6 | |
| 7 | struct ufdt *ufdt_construct(void *fdtp) { |
| 8 | struct ufdt *res_ufdt = dto_malloc(sizeof(struct ufdt)); |
| 9 | res_ufdt->fdtp = fdtp; |
| 10 | res_ufdt->root = NULL; |
| 11 | |
| 12 | return res_ufdt; |
| 13 | } |
| 14 | |
| 15 | void ufdt_destruct(struct ufdt *tree) { |
| 16 | ufdt_node_destruct(tree->root); |
| 17 | dto_free(tree->phandle_table.data); |
| 18 | } |
| 19 | |
| 20 | static struct ufdt_node *ufdt_new_node(void *fdtp, int node_offset) { |
| 21 | if (fdtp == NULL) { |
| 22 | dto_error("Failed to get new_node because tree is NULL\n"); |
| 23 | return NULL; |
| 24 | } |
| 25 | |
| 26 | fdt32_t *fdt_tag_ptr = |
| 27 | (fdt32_t *)fdt_offset_ptr(fdtp, node_offset, sizeof(fdt32_t)); |
| 28 | struct ufdt_node *res = ufdt_node_construct(fdtp, fdt_tag_ptr); |
| 29 | return res; |
| 30 | } |
| 31 | |
| 32 | static struct ufdt_node *fdt_to_ufdt_tree(void *fdtp, int cur_fdt_tag_offset, |
| 33 | int *next_fdt_tag_offset, |
| 34 | int cur_tag) { |
| 35 | if (fdtp == NULL) { |
| 36 | return NULL; |
| 37 | } |
| 38 | uint32_t tag; |
| 39 | struct ufdt_node *res, *child_node; |
| 40 | |
| 41 | res = NULL; |
| 42 | child_node = NULL; |
| 43 | tag = cur_tag; |
| 44 | |
| 45 | switch (tag) { |
| 46 | case FDT_END_NODE: |
| 47 | case FDT_NOP: |
| 48 | case FDT_END: |
| 49 | break; |
| 50 | |
| 51 | case FDT_PROP: |
| 52 | res = ufdt_new_node(fdtp, cur_fdt_tag_offset); |
| 53 | break; |
| 54 | |
| 55 | case FDT_BEGIN_NODE: |
| 56 | res = ufdt_new_node(fdtp, cur_fdt_tag_offset); |
| 57 | |
| 58 | do { |
| 59 | cur_fdt_tag_offset = *next_fdt_tag_offset; |
| 60 | tag = fdt_next_tag(fdtp, cur_fdt_tag_offset, next_fdt_tag_offset); |
| 61 | child_node = fdt_to_ufdt_tree(fdtp, cur_fdt_tag_offset, |
| 62 | next_fdt_tag_offset, tag); |
| 63 | ufdt_node_add_child(res, child_node); |
| 64 | } while (tag != FDT_END_NODE); |
| 65 | break; |
| 66 | |
| 67 | default: |
| 68 | break; |
| 69 | } |
| 70 | |
| 71 | return res; |
| 72 | } |
| 73 | |
| 74 | void ufdt_print(struct ufdt *tree) { ufdt_node_print(tree->root, 0); } |
| 75 | |
| 76 | struct ufdt_node *ufdt_get_node_by_path_len(struct ufdt *tree, const char *path, |
| 77 | int len) { |
| 78 | /* |
| 79 | * RARE: aliases |
| 80 | * In device tree, we can assign some alias to specific nodes by defining |
| 81 | * these relation in "/aliases" node. |
| 82 | * The node has the form: |
| 83 | * { |
| 84 | * a = "/a_for_apple"; |
| 85 | * b = "/b_for_banana"; |
| 86 | * }; |
| 87 | * So the path "a/subnode_1" should be expanded to "/a_for_apple/subnode_1". |
| 88 | */ |
| 89 | if (*path != '/') { |
| 90 | const char *end = path + len; |
| 91 | |
| 92 | const char *next_slash; |
| 93 | next_slash = dto_memchr(path, '/', end - path); |
| 94 | if (!next_slash) next_slash = end; |
| 95 | |
| 96 | struct ufdt_node *aliases_node = |
| 97 | ufdt_node_get_node_by_path(tree->root, "/aliases"); |
| 98 | aliases_node = ufdt_node_get_property_by_name_len(aliases_node, path, |
| 99 | next_slash - path); |
| 100 | |
| 101 | int path_len = 0; |
| 102 | const char *alias_path = |
| 103 | ufdt_node_get_fdt_prop_data(aliases_node, &path_len); |
| 104 | |
| 105 | if (alias_path == NULL) { |
| 106 | dto_error("Failed to find alias %s\n", path); |
| 107 | return NULL; |
| 108 | } |
| 109 | |
| 110 | struct ufdt_node *target_node = |
| 111 | ufdt_node_get_node_by_path_len(tree->root, alias_path, path_len); |
| 112 | |
| 113 | return ufdt_node_get_node_by_path_len(target_node, next_slash, |
| 114 | end - next_slash); |
| 115 | } |
| 116 | return ufdt_node_get_node_by_path_len(tree->root, path, len); |
| 117 | } |
| 118 | |
| 119 | struct ufdt_node *ufdt_get_node_by_path(struct ufdt *tree, const char *path) { |
| 120 | return ufdt_get_node_by_path_len(tree, path, dto_strlen(path)); |
| 121 | } |
| 122 | |
| 123 | struct ufdt_node *ufdt_get_node_by_phandle(struct ufdt *tree, |
| 124 | uint32_t phandle) { |
| 125 | struct ufdt_node *res = NULL; |
| 126 | /* |
| 127 | * Do binary search in phandle_table.data. |
| 128 | * [s, e) means the possible range which contains target node. |
| 129 | */ |
| 130 | int s = 0, e = tree->phandle_table.len; |
| 131 | while (e - s > 1) { |
| 132 | int mid = s + ((e - s) >> 1); |
| 133 | uint32_t mid_phandle = tree->phandle_table.data[mid].phandle; |
| 134 | if (phandle < mid_phandle) |
| 135 | e = mid; |
| 136 | else |
| 137 | s = mid; |
| 138 | } |
| 139 | if (e - s > 0) { |
| 140 | res = tree->phandle_table.data[s].node; |
| 141 | } |
| 142 | return res; |
| 143 | } |
| 144 | |
| 145 | int merge_children(struct ufdt_node *node_a, struct ufdt_node *node_b) { |
| 146 | int err = 0; |
| 147 | struct ufdt_node *it; |
| 148 | for (it = ((struct fdt_node_ufdt_node *)node_b)->child; it;) { |
| 149 | struct ufdt_node *cur_node = it; |
| 150 | it = it->sibling; |
| 151 | cur_node->sibling = NULL; |
| 152 | struct ufdt_node *target_node = NULL; |
| 153 | if (tag_of(cur_node) == FDT_BEGIN_NODE) { |
| 154 | target_node = ufdt_node_get_subnode_by_name(node_a, name_of(cur_node)); |
| 155 | } else { |
| 156 | target_node = ufdt_node_get_property_by_name(node_a, name_of(cur_node)); |
| 157 | } |
| 158 | if (target_node == NULL) { |
| 159 | err = ufdt_node_add_child(node_a, cur_node); |
| 160 | } else { |
| 161 | err = merge_ufdt_into(target_node, cur_node); |
| 162 | } |
| 163 | if (err < 0) return -1; |
| 164 | } |
| 165 | /* |
| 166 | * The ufdt_node* in node_b will be copied to node_a. |
| 167 | * To prevent the ufdt_node from being freed twice |
| 168 | * (main_tree and overlay_tree) at the end of function |
| 169 | * ufdt_apply_overlay(), set this node in node_b |
| 170 | * (overlay_tree) to NULL. |
| 171 | */ |
| 172 | ((struct fdt_node_ufdt_node *)node_b)->child = NULL; |
| 173 | |
| 174 | return 0; |
| 175 | } |
| 176 | |
| 177 | int merge_ufdt_into(struct ufdt_node *node_a, struct ufdt_node *node_b) { |
| 178 | if (tag_of(node_a) == FDT_PROP) { |
| 179 | node_a->fdt_tag_ptr = node_b->fdt_tag_ptr; |
| 180 | return 0; |
| 181 | } |
| 182 | |
| 183 | int err = 0; |
| 184 | err = merge_children(node_a, node_b); |
| 185 | if (err < 0) return -1; |
| 186 | |
| 187 | return 0; |
| 188 | } |
| 189 | |
| 190 | void ufdt_map(struct ufdt *tree, struct ufdt_node_closure closure) { |
| 191 | ufdt_node_map(tree->root, closure); |
| 192 | } |
| 193 | |
| 194 | static int count_phandle_node(struct ufdt_node *node) { |
| 195 | if (node == NULL) return 0; |
| 196 | if (tag_of(node) != FDT_BEGIN_NODE) return 0; |
| 197 | int res = 0; |
| 198 | if (ufdt_node_get_phandle(node) > 0) res++; |
| 199 | struct ufdt_node **it; |
| 200 | for_each_child(it, node) { res += count_phandle_node(*it); } |
| 201 | return res; |
| 202 | } |
| 203 | |
| 204 | static void set_phandle_table_entry(struct ufdt_node *node, |
| 205 | struct phandle_table_entry *data, |
| 206 | int *cur) { |
| 207 | if (node == NULL || tag_of(node) != FDT_BEGIN_NODE) return; |
| 208 | int ph = ufdt_node_get_phandle(node); |
| 209 | if (ph > 0) { |
| 210 | data[*cur].phandle = ph; |
| 211 | data[*cur].node = node; |
| 212 | (*cur)++; |
| 213 | } |
| 214 | struct ufdt_node **it; |
| 215 | for_each_node(it, node) set_phandle_table_entry(*it, data, cur); |
| 216 | return; |
| 217 | } |
| 218 | |
| 219 | int phandle_table_entry_cmp(const void *pa, const void *pb) { |
| 220 | uint32_t ph_a = ((const struct phandle_table_entry *)pa)->phandle; |
| 221 | uint32_t ph_b = ((const struct phandle_table_entry *)pb)->phandle; |
| 222 | if (ph_a < ph_b) |
| 223 | return -1; |
| 224 | else if (ph_a == ph_b) |
| 225 | return 0; |
| 226 | else |
| 227 | return 1; |
| 228 | } |
| 229 | |
| 230 | struct static_phandle_table build_phandle_table(struct ufdt *tree) { |
| 231 | struct static_phandle_table res; |
| 232 | res.len = count_phandle_node(tree->root); |
| 233 | res.data = dto_malloc(sizeof(struct phandle_table_entry) * res.len); |
| 234 | int cur = 0; |
| 235 | set_phandle_table_entry(tree->root, res.data, &cur); |
| 236 | dto_qsort(res.data, res.len, sizeof(struct phandle_table_entry), |
| 237 | phandle_table_entry_cmp); |
| 238 | return res; |
| 239 | } |
| 240 | |
| 241 | struct ufdt *fdt_to_ufdt(void *fdtp, size_t fdt_size) { |
| 242 | (void)(fdt_size); // unused parameter |
| 243 | |
| 244 | struct ufdt *res_tree = ufdt_construct(fdtp); |
| 245 | |
| 246 | int start_offset = fdt_path_offset(fdtp, "/"); |
| 247 | if (start_offset < 0) { |
| 248 | res_tree->fdtp = NULL; |
| 249 | return res_tree; |
| 250 | } |
| 251 | |
| 252 | int end_offset; |
| 253 | int start_tag = fdt_next_tag(fdtp, start_offset, &end_offset); |
| 254 | res_tree->root = fdt_to_ufdt_tree(fdtp, start_offset, &end_offset, start_tag); |
| 255 | |
| 256 | res_tree->phandle_table = build_phandle_table(res_tree); |
| 257 | |
| 258 | return res_tree; |
| 259 | } |
| 260 | |
| 261 | int ufdt_to_fdt(struct ufdt *tree, void *buf, int buf_size) { |
| 262 | int err; |
| 263 | err = fdt_create(buf, buf_size); |
| 264 | if (err < 0) return -1; |
| 265 | |
| 266 | int n_mem_rsv = fdt_num_mem_rsv(tree->fdtp); |
| 267 | for (int i = 0; i < n_mem_rsv; i++) { |
| 268 | uint64_t addr, size; |
| 269 | fdt_get_mem_rsv(tree->fdtp, i, &addr, &size); |
| 270 | fdt_add_reservemap_entry(buf, addr, size); |
| 271 | } |
| 272 | |
| 273 | err = fdt_finish_reservemap(buf); |
| 274 | if (err < 0) return -1; |
| 275 | |
| 276 | /* |
| 277 | * Obtains all props for later use because getting them from |
| 278 | * FDT requires complicated manipulation. |
| 279 | */ |
| 280 | struct ufdt_node_dict all_props = ufdt_node_dict_construct(); |
| 281 | err = output_ufdt_node_to_fdt(tree->root, buf, &all_props); |
| 282 | if (err < 0) return -1; |
| 283 | |
| 284 | ufdt_node_dict_destruct(&all_props); |
| 285 | |
| 286 | err = fdt_finish(buf); |
| 287 | if (err < 0) return -1; |
| 288 | |
| 289 | /* |
| 290 | * IMPORTANT: fdt_totalsize(buf) might be less than buf_size |
| 291 | * so this is needed to make use of remain spaces. |
| 292 | */ |
| 293 | return fdt_open_into(buf, buf, buf_size); |
| 294 | } |