1 /* 2 * Copyright (C) 2016 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 #include "libufdt.h" 18 19 #include "ufdt_prop_dict.h" 20 21 struct ufdt *ufdt_construct(void *fdtp) { 22 /* Inital size is 2, will be exponentially increased when it needed later. 23 (2 -> 4 -> 8 -> ...) */ 24 const int DEFAULT_MEM_SIZE_FDTPS = 2; 25 26 void **fdtps = NULL; 27 struct ufdt *res_ufdt = NULL; 28 29 fdtps = (void **)dto_malloc(sizeof(void *) * DEFAULT_MEM_SIZE_FDTPS); 30 if (fdtps == NULL) goto error; 31 fdtps[0] = fdtp; 32 33 res_ufdt = dto_malloc(sizeof(struct ufdt)); 34 if (res_ufdt == NULL) goto error; 35 36 res_ufdt->fdtps = fdtps; 37 res_ufdt->mem_size_fdtps = DEFAULT_MEM_SIZE_FDTPS; 38 res_ufdt->num_used_fdtps = (fdtp != NULL ? 1 : 0); 39 res_ufdt->root = NULL; 40 41 return res_ufdt; 42 43 error: 44 if (res_ufdt) dto_free(res_ufdt); 45 if (fdtps) dto_free(fdtps); 46 47 return NULL; 48 } 49 50 void ufdt_destruct(struct ufdt *tree) { 51 if (tree == NULL) return; 52 53 ufdt_node_destruct(tree->root); 54 55 dto_free(tree->fdtps); 56 dto_free(tree->phandle_table.data); 57 dto_free(tree); 58 } 59 60 int ufdt_add_fdt(struct ufdt *tree, void *fdtp) { 61 if (fdtp == NULL) { 62 return -1; 63 } 64 65 int i = tree->num_used_fdtps; 66 if (i >= tree->mem_size_fdtps) { 67 int new_size = tree->mem_size_fdtps * 2; 68 void **new_fdtps = dto_malloc(sizeof(void *) * new_size); 69 if (new_fdtps == NULL) return -1; 70 71 dto_memcpy(new_fdtps, tree->fdtps, sizeof(void *) * tree->mem_size_fdtps); 72 dto_free(tree->fdtps); 73 74 tree->fdtps = new_fdtps; 75 tree->mem_size_fdtps = new_size; 76 } 77 78 tree->fdtps[i] = fdtp; 79 tree->num_used_fdtps = i + 1; 80 81 return 0; 82 } 83 84 int ufdt_get_string_off(const struct ufdt *tree, const char *s) { 85 /* fdt_create() sets the dt_string_off to the end of fdt buffer, 86 and _ufdt_output_strtab_to_fdt() copy all string tables in reversed order. 87 So, here the return offset value is base on the end of all string buffers, 88 and it should be a minus value. */ 89 int res_off = 0; 90 for (int i = 0; i < tree->num_used_fdtps; i++) { 91 void *fdt = tree->fdtps[i]; 92 const char *strtab_start = (const char *)fdt + fdt_off_dt_strings(fdt); 93 int strtab_size = fdt_size_dt_strings(fdt); 94 const char *strtab_end = strtab_start + strtab_size; 95 96 /* Check if the string is in the string table */ 97 if (s >= strtab_start && s < strtab_end) { 98 res_off += (s - strtab_end); 99 return res_off; 100 } 101 102 res_off -= strtab_size; 103 } 104 /* Can not find the string, return 0 */ 105 return 0; 106 } 107 108 static struct ufdt_node *ufdt_new_node(void *fdtp, int node_offset) { 109 if (fdtp == NULL) { 110 dto_error("Failed to get new_node because tree is NULL\n"); 111 return NULL; 112 } 113 114 fdt32_t *fdt_tag_ptr = 115 (fdt32_t *)fdt_offset_ptr(fdtp, node_offset, sizeof(fdt32_t)); 116 struct ufdt_node *res = ufdt_node_construct(fdtp, fdt_tag_ptr); 117 return res; 118 } 119 120 static struct ufdt_node *fdt_to_ufdt_tree(void *fdtp, int cur_fdt_tag_offset, 121 int *next_fdt_tag_offset, 122 int cur_tag) { 123 if (fdtp == NULL) { 124 return NULL; 125 } 126 uint32_t tag; 127 struct ufdt_node *res, *child_node; 128 129 res = NULL; 130 child_node = NULL; 131 tag = cur_tag; 132 133 switch (tag) { 134 case FDT_END_NODE: 135 case FDT_NOP: 136 case FDT_END: 137 break; 138 139 case FDT_PROP: 140 res = ufdt_new_node(fdtp, cur_fdt_tag_offset); 141 break; 142 143 case FDT_BEGIN_NODE: 144 res = ufdt_new_node(fdtp, cur_fdt_tag_offset); 145 146 do { 147 cur_fdt_tag_offset = *next_fdt_tag_offset; 148 tag = fdt_next_tag(fdtp, cur_fdt_tag_offset, next_fdt_tag_offset); 149 child_node = fdt_to_ufdt_tree(fdtp, cur_fdt_tag_offset, 150 next_fdt_tag_offset, tag); 151 ufdt_node_add_child(res, child_node); 152 } while (tag != FDT_END_NODE); 153 break; 154 155 default: 156 break; 157 } 158 159 return res; 160 } 161 162 void ufdt_print(struct ufdt *tree) { 163 ufdt_node_print(tree->root, 0); 164 } 165 166 struct ufdt_node *ufdt_get_node_by_path_len(struct ufdt *tree, const char *path, 167 int len) { 168 /* 169 * RARE: aliases 170 * In device tree, we can assign some alias to specific nodes by defining 171 * these relation in "/aliases" node. 172 * The node has the form: 173 * { 174 * a = "/a_for_apple"; 175 * b = "/b_for_banana"; 176 * }; 177 * So the path "a/subnode_1" should be expanded to "/a_for_apple/subnode_1". 178 */ 179 if (*path != '/') { 180 const char *end = path + len; 181 182 const char *next_slash; 183 next_slash = dto_memchr(path, '/', end - path); 184 if (!next_slash) next_slash = end; 185 186 struct ufdt_node *aliases_node = 187 ufdt_node_get_node_by_path(tree->root, "/aliases"); 188 aliases_node = ufdt_node_get_property_by_name_len(aliases_node, path, 189 next_slash - path); 190 191 int path_len = 0; 192 const char *alias_path = 193 ufdt_node_get_fdt_prop_data(aliases_node, &path_len); 194 195 if (alias_path == NULL) { 196 dto_error("Failed to find alias %s\n", path); 197 return NULL; 198 } 199 200 struct ufdt_node *target_node = 201 ufdt_node_get_node_by_path_len(tree->root, alias_path, path_len); 202 203 return ufdt_node_get_node_by_path_len(target_node, next_slash, 204 end - next_slash); 205 } 206 return ufdt_node_get_node_by_path_len(tree->root, path, len); 207 } 208 209 struct ufdt_node *ufdt_get_node_by_path(struct ufdt *tree, const char *path) { 210 return ufdt_get_node_by_path_len(tree, path, dto_strlen(path)); 211 } 212 213 struct ufdt_node *ufdt_get_node_by_phandle(struct ufdt *tree, 214 uint32_t phandle) { 215 struct ufdt_node *res = NULL; 216 /* 217 * Do binary search in phandle_table.data. 218 * [s, e) means the possible range which contains target node. 219 */ 220 int s = 0, e = tree->phandle_table.len; 221 while (e - s > 1) { 222 int mid = s + ((e - s) >> 1); 223 uint32_t mid_phandle = tree->phandle_table.data[mid].phandle; 224 if (phandle < mid_phandle) 225 e = mid; 226 else 227 s = mid; 228 } 229 if (e - s > 0) { 230 res = tree->phandle_table.data[s].node; 231 } 232 return res; 233 } 234 235 int merge_children(struct ufdt_node *node_a, struct ufdt_node *node_b) { 236 int err = 0; 237 struct ufdt_node *it; 238 for (it = ((struct fdt_node_ufdt_node *)node_b)->child; it;) { 239 struct ufdt_node *cur_node = it; 240 it = it->sibling; 241 cur_node->sibling = NULL; 242 struct ufdt_node *target_node = NULL; 243 if (tag_of(cur_node) == FDT_BEGIN_NODE) { 244 target_node = ufdt_node_get_subnode_by_name(node_a, name_of(cur_node)); 245 } else { 246 target_node = ufdt_node_get_property_by_name(node_a, name_of(cur_node)); 247 } 248 if (target_node == NULL) { 249 err = ufdt_node_add_child(node_a, cur_node); 250 } else { 251 err = merge_ufdt_into(target_node, cur_node); 252 dto_free(cur_node); 253 } 254 if (err < 0) return -1; 255 } 256 /* 257 * The ufdt_node* in node_b will be copied to node_a. 258 * To prevent the ufdt_node from being freed twice 259 * (main_tree and overlay_tree) at the end of function 260 * ufdt_apply_overlay(), set this node in node_b 261 * (overlay_tree) to NULL. 262 */ 263 ((struct fdt_node_ufdt_node *)node_b)->child = NULL; 264 265 return 0; 266 } 267 268 int merge_ufdt_into(struct ufdt_node *node_a, struct ufdt_node *node_b) { 269 if (tag_of(node_a) == FDT_PROP) { 270 node_a->fdt_tag_ptr = node_b->fdt_tag_ptr; 271 return 0; 272 } 273 274 int err = 0; 275 err = merge_children(node_a, node_b); 276 if (err < 0) return -1; 277 278 return 0; 279 } 280 281 void ufdt_map(struct ufdt *tree, struct ufdt_node_closure closure) { 282 ufdt_node_map(tree->root, closure); 283 } 284 285 static int count_phandle_node(struct ufdt_node *node) { 286 if (node == NULL) return 0; 287 if (tag_of(node) != FDT_BEGIN_NODE) return 0; 288 int res = 0; 289 if (ufdt_node_get_phandle(node) > 0) res++; 290 struct ufdt_node **it; 291 for_each_child(it, node) { res += count_phandle_node(*it); } 292 return res; 293 } 294 295 static void set_phandle_table_entry(struct ufdt_node *node, 296 struct phandle_table_entry *data, 297 int *cur) { 298 if (node == NULL || tag_of(node) != FDT_BEGIN_NODE) return; 299 int ph = ufdt_node_get_phandle(node); 300 if (ph > 0) { 301 data[*cur].phandle = ph; 302 data[*cur].node = node; 303 (*cur)++; 304 } 305 struct ufdt_node **it; 306 for_each_node(it, node) set_phandle_table_entry(*it, data, cur); 307 return; 308 } 309 310 int phandle_table_entry_cmp(const void *pa, const void *pb) { 311 uint32_t ph_a = ((const struct phandle_table_entry *)pa)->phandle; 312 uint32_t ph_b = ((const struct phandle_table_entry *)pb)->phandle; 313 if (ph_a < ph_b) 314 return -1; 315 else if (ph_a == ph_b) 316 return 0; 317 else 318 return 1; 319 } 320 321 struct static_phandle_table build_phandle_table(struct ufdt *tree) { 322 struct static_phandle_table res; 323 res.len = count_phandle_node(tree->root); 324 res.data = dto_malloc(sizeof(struct phandle_table_entry) * res.len); 325 int cur = 0; 326 set_phandle_table_entry(tree->root, res.data, &cur); 327 dto_qsort(res.data, res.len, sizeof(struct phandle_table_entry), 328 phandle_table_entry_cmp); 329 return res; 330 } 331 332 struct ufdt *fdt_to_ufdt(void *fdtp, size_t fdt_size) { 333 (void)(fdt_size); /* unused parameter */ 334 335 int start_offset = fdt_path_offset(fdtp, "/"); 336 if (start_offset < 0) { 337 return ufdt_construct(NULL); 338 } 339 340 struct ufdt *res_tree = ufdt_construct(fdtp); 341 int end_offset; 342 int start_tag = fdt_next_tag(fdtp, start_offset, &end_offset); 343 res_tree->root = fdt_to_ufdt_tree(fdtp, start_offset, &end_offset, start_tag); 344 345 res_tree->phandle_table = build_phandle_table(res_tree); 346 347 return res_tree; 348 } 349 350 static int _ufdt_get_property_nameoff(const struct ufdt *tree, const char *name, 351 const struct ufdt_prop_dict *dict) { 352 int res; 353 const struct fdt_property *same_name_prop = ufdt_prop_dict_find(dict, name); 354 if (same_name_prop != NULL) { 355 /* There is a property with same name, just use its string offset */ 356 res = fdt32_to_cpu(same_name_prop->nameoff); 357 } else { 358 /* Get the string offset from the string table of the current tree */ 359 res = ufdt_get_string_off(tree, name); 360 if (res == 0) { 361 dto_error("Cannot find property name in string table: %s\n", name); 362 return 0; 363 } 364 } 365 return res; 366 } 367 368 static int _ufdt_output_property_to_fdt( 369 const struct ufdt *tree, void *fdtp, 370 const struct fdt_prop_ufdt_node *prop_node, struct ufdt_prop_dict *dict) { 371 int nameoff = _ufdt_get_property_nameoff(tree, prop_node->name, dict); 372 if (nameoff == 0) return -1; 373 374 int data_len = 0; 375 void *data = ufdt_node_get_fdt_prop_data(&prop_node->parent, &data_len); 376 int aligned_data_len = (data_len + (FDT_TAGSIZE - 1)) & ~(FDT_TAGSIZE - 1); 377 378 int new_propoff = fdt_size_dt_struct(fdtp); 379 int new_prop_size = sizeof(struct fdt_property) + aligned_data_len; 380 struct fdt_property *new_prop = 381 (struct fdt_property *)((char *)fdtp + fdt_off_dt_struct(fdtp) + 382 new_propoff); 383 char *fdt_end = (char *)fdtp + fdt_totalsize(fdtp); 384 if ((char *)new_prop + new_prop_size > fdt_end) { 385 dto_error("Not enough space for adding property.\n"); 386 return -1; 387 } 388 fdt_set_size_dt_struct(fdtp, new_propoff + new_prop_size); 389 390 new_prop->tag = cpu_to_fdt32(FDT_PROP); 391 new_prop->nameoff = cpu_to_fdt32(nameoff); 392 new_prop->len = cpu_to_fdt32(data_len); 393 dto_memcpy(new_prop->data, data, data_len); 394 395 ufdt_prop_dict_add(dict, new_prop); 396 397 return 0; 398 } 399 400 static int _ufdt_output_node_to_fdt(const struct ufdt *tree, void *fdtp, 401 const struct ufdt_node *node, 402 struct ufdt_prop_dict *dict) { 403 uint32_t tag = tag_of(node); 404 405 if (tag == FDT_PROP) { 406 return _ufdt_output_property_to_fdt( 407 tree, fdtp, (const struct fdt_prop_ufdt_node *)node, dict); 408 } 409 410 int err = fdt_begin_node(fdtp, name_of(node)); 411 if (err < 0) return -1; 412 413 struct ufdt_node **it; 414 for_each_prop(it, node) { 415 err = _ufdt_output_node_to_fdt(tree, fdtp, *it, dict); 416 if (err < 0) return -1; 417 } 418 419 for_each_node(it, node) { 420 err = _ufdt_output_node_to_fdt(tree, fdtp, *it, dict); 421 if (err < 0) return -1; 422 } 423 424 err = fdt_end_node(fdtp); 425 if (err < 0) return -1; 426 427 return 0; 428 } 429 430 static int _ufdt_output_strtab_to_fdt(const struct ufdt *tree, void *fdt) { 431 /* Currently, we don't know the final dt_struct size, so we copy all 432 string tables to the end of the target fdt buffer in reversed order. 433 At last, fdt_finish() will adjust dt_string offset */ 434 const char *struct_top = 435 (char *)fdt + fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt); 436 char *dest = (char *)fdt + fdt_totalsize(fdt); 437 438 int dest_size = 0; 439 for (int i = 0; i < tree->num_used_fdtps; i++) { 440 void *src_fdt = tree->fdtps[i]; 441 const char *src_strtab = (const char *)src_fdt + fdt_off_dt_strings(src_fdt); 442 int strtab_size = fdt_size_dt_strings(src_fdt); 443 444 dest -= strtab_size; 445 if (dest < struct_top) { 446 dto_error("Not enough space for string table.\n"); 447 return -1; 448 } 449 450 dto_memcpy(dest, src_strtab, strtab_size); 451 452 dest_size += strtab_size; 453 } 454 455 fdt_set_size_dt_strings(fdt, dest_size); 456 457 return 0; 458 } 459 460 int ufdt_to_fdt(const struct ufdt *tree, void *buf, int buf_size) { 461 if (tree->num_used_fdtps == 0) return -1; 462 463 int err; 464 err = fdt_create(buf, buf_size); 465 if (err < 0) return -1; 466 467 /* Here we output the memory reserve map of the ONLY FIRST fdt, 468 to be in compliance with the DTO behavior of libfdt. */ 469 int n_mem_rsv = fdt_num_mem_rsv(tree->fdtps[0]); 470 for (int i = 0; i < n_mem_rsv; i++) { 471 uint64_t addr, size; 472 fdt_get_mem_rsv(tree->fdtps[0], i, &addr, &size); 473 fdt_add_reservemap_entry(buf, addr, size); 474 } 475 476 err = fdt_finish_reservemap(buf); 477 if (err < 0) return -1; 478 479 err = _ufdt_output_strtab_to_fdt(tree, buf); 480 if (err < 0) return -1; 481 482 struct ufdt_prop_dict dict; 483 err = ufdt_prop_dict_construct(&dict, buf); 484 if (err < 0) return -1; 485 486 err = _ufdt_output_node_to_fdt(tree, buf, tree->root, &dict); 487 if (err < 0) return -1; 488 489 ufdt_prop_dict_destruct(&dict); 490 491 err = fdt_finish(buf); 492 if (err < 0) return -1; 493 494 /* 495 * IMPORTANT: fdt_totalsize(buf) might be less than buf_size 496 * so this is needed to make use of remain spaces. 497 */ 498 return fdt_open_into(buf, buf, buf_size); 499 } 500