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