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