SzuWei Lin | 6ad4caa | 2017-04-06 17:12:27 +0800 | [diff] [blame] | 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 | |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 17 | #include "libufdt.h" |
| 18 | |
SzuWei Lin | d62a849 | 2017-04-24 10:17:15 +0800 | [diff] [blame] | 19 | #include "ufdt_node_pool.h" |
SzuWei Lin | 1be68ae | 2017-03-30 11:44:54 +0800 | [diff] [blame] | 20 | #include "ufdt_prop_dict.h" |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 21 | |
SzuWei Lin | d62a849 | 2017-04-24 10:17:15 +0800 | [diff] [blame] | 22 | struct ufdt *ufdt_construct(void *fdtp, struct ufdt_node_pool *pool) { |
| 23 | (void)(pool); /* unused parameter */ |
| 24 | |
SzuWei Lin | 1be68ae | 2017-03-30 11:44:54 +0800 | [diff] [blame] | 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); |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 42 | res_ufdt->root = NULL; |
| 43 | |
| 44 | return res_ufdt; |
SzuWei Lin | 1be68ae | 2017-03-30 11:44:54 +0800 | [diff] [blame] | 45 | |
| 46 | error: |
| 47 | if (res_ufdt) dto_free(res_ufdt); |
| 48 | if (fdtps) dto_free(fdtps); |
| 49 | |
| 50 | return NULL; |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 51 | } |
| 52 | |
SzuWei Lin | d62a849 | 2017-04-24 10:17:15 +0800 | [diff] [blame] | 53 | void ufdt_destruct(struct ufdt *tree, struct ufdt_node_pool *pool) { |
SzuWei Lin | 1be68ae | 2017-03-30 11:44:54 +0800 | [diff] [blame] | 54 | if (tree == NULL) return; |
| 55 | |
SzuWei Lin | d62a849 | 2017-04-24 10:17:15 +0800 | [diff] [blame] | 56 | ufdt_node_destruct(tree->root, pool); |
SzuWei Lin | 1be68ae | 2017-03-30 11:44:54 +0800 | [diff] [blame] | 57 | |
| 58 | dto_free(tree->fdtps); |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 59 | dto_free(tree->phandle_table.data); |
SzuWei Lin | 70107c8 | 2017-04-07 18:56:13 +0800 | [diff] [blame] | 60 | dto_free(tree); |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 61 | } |
| 62 | |
SzuWei Lin | 1be68ae | 2017-03-30 11:44:54 +0800 | [diff] [blame] | 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 | |
SzuWei Lin | d62a849 | 2017-04-24 10:17:15 +0800 | [diff] [blame] | 111 | static struct ufdt_node *ufdt_new_node(void *fdtp, int node_offset, |
| 112 | struct ufdt_node_pool *pool) { |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 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)); |
SzuWei Lin | d62a849 | 2017-04-24 10:17:15 +0800 | [diff] [blame] | 120 | struct ufdt_node *res = ufdt_node_construct(fdtp, fdt_tag_ptr, pool); |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 121 | return res; |
| 122 | } |
| 123 | |
| 124 | static struct ufdt_node *fdt_to_ufdt_tree(void *fdtp, int cur_fdt_tag_offset, |
SzuWei Lin | d62a849 | 2017-04-24 10:17:15 +0800 | [diff] [blame] | 125 | int *next_fdt_tag_offset, int cur_tag, |
| 126 | struct ufdt_node_pool *pool) { |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 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: |
SzuWei Lin | d62a849 | 2017-04-24 10:17:15 +0800 | [diff] [blame] | 144 | res = ufdt_new_node(fdtp, cur_fdt_tag_offset, pool); |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 145 | break; |
| 146 | |
| 147 | case FDT_BEGIN_NODE: |
SzuWei Lin | d62a849 | 2017-04-24 10:17:15 +0800 | [diff] [blame] | 148 | res = ufdt_new_node(fdtp, cur_fdt_tag_offset, pool); |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 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, |
SzuWei Lin | d62a849 | 2017-04-24 10:17:15 +0800 | [diff] [blame] | 154 | next_fdt_tag_offset, tag, pool); |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 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 | |
SzuWei Lin | 1be68ae | 2017-03-30 11:44:54 +0800 | [diff] [blame] | 166 | void ufdt_print(struct ufdt *tree) { |
| 167 | ufdt_node_print(tree->root, 0); |
| 168 | } |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 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 | |
Mike McTernan | 234b637 | 2022-09-14 15:21:40 +0100 | [diff] [blame] | 199 | if (alias_path == NULL || path_len == 0) { |
| 200 | dto_error("Failed to find valid alias %s\n", path); |
| 201 | return NULL; |
| 202 | } |
| 203 | |
| 204 | /* property data must be a nul terminated string */ |
| 205 | int alias_len = strnlen(alias_path, path_len); |
| 206 | |
| 207 | if (alias_len != path_len - 1 || alias_len == 0) { |
| 208 | dto_error("Invalid alias for %s\n", path); |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 209 | return NULL; |
| 210 | } |
| 211 | |
| 212 | struct ufdt_node *target_node = |
Mike McTernan | 234b637 | 2022-09-14 15:21:40 +0100 | [diff] [blame] | 213 | ufdt_node_get_node_by_path_len(tree->root, alias_path, alias_len); |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 214 | |
| 215 | return ufdt_node_get_node_by_path_len(target_node, next_slash, |
| 216 | end - next_slash); |
| 217 | } |
| 218 | return ufdt_node_get_node_by_path_len(tree->root, path, len); |
| 219 | } |
| 220 | |
| 221 | struct ufdt_node *ufdt_get_node_by_path(struct ufdt *tree, const char *path) { |
| 222 | return ufdt_get_node_by_path_len(tree, path, dto_strlen(path)); |
| 223 | } |
| 224 | |
| 225 | struct ufdt_node *ufdt_get_node_by_phandle(struct ufdt *tree, |
| 226 | uint32_t phandle) { |
| 227 | struct ufdt_node *res = NULL; |
| 228 | /* |
| 229 | * Do binary search in phandle_table.data. |
| 230 | * [s, e) means the possible range which contains target node. |
| 231 | */ |
| 232 | int s = 0, e = tree->phandle_table.len; |
| 233 | while (e - s > 1) { |
| 234 | int mid = s + ((e - s) >> 1); |
| 235 | uint32_t mid_phandle = tree->phandle_table.data[mid].phandle; |
| 236 | if (phandle < mid_phandle) |
| 237 | e = mid; |
| 238 | else |
| 239 | s = mid; |
| 240 | } |
Hridya Valsaraju | 9ec4bda | 2018-01-29 14:11:39 -0800 | [diff] [blame] | 241 | if (e - s > 0 && tree->phandle_table.data[s].phandle == phandle) { |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 242 | res = tree->phandle_table.data[s].node; |
| 243 | } |
| 244 | return res; |
| 245 | } |
| 246 | |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 247 | static int count_phandle_node(struct ufdt_node *node) { |
| 248 | if (node == NULL) return 0; |
SzuWei Lin | 8a7039c | 2017-04-14 15:38:15 +0800 | [diff] [blame] | 249 | if (ufdt_node_tag(node) != FDT_BEGIN_NODE) return 0; |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 250 | int res = 0; |
| 251 | if (ufdt_node_get_phandle(node) > 0) res++; |
| 252 | struct ufdt_node **it; |
| 253 | for_each_child(it, node) { res += count_phandle_node(*it); } |
| 254 | return res; |
| 255 | } |
| 256 | |
| 257 | static void set_phandle_table_entry(struct ufdt_node *node, |
SzuWei Lin | 8a7039c | 2017-04-14 15:38:15 +0800 | [diff] [blame] | 258 | struct ufdt_phandle_table_entry *data, |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 259 | int *cur) { |
SzuWei Lin | 8a7039c | 2017-04-14 15:38:15 +0800 | [diff] [blame] | 260 | if (node == NULL || ufdt_node_tag(node) != FDT_BEGIN_NODE) return; |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 261 | int ph = ufdt_node_get_phandle(node); |
| 262 | if (ph > 0) { |
| 263 | data[*cur].phandle = ph; |
| 264 | data[*cur].node = node; |
| 265 | (*cur)++; |
| 266 | } |
| 267 | struct ufdt_node **it; |
| 268 | for_each_node(it, node) set_phandle_table_entry(*it, data, cur); |
| 269 | return; |
| 270 | } |
| 271 | |
| 272 | int phandle_table_entry_cmp(const void *pa, const void *pb) { |
SzuWei Lin | 8a7039c | 2017-04-14 15:38:15 +0800 | [diff] [blame] | 273 | uint32_t ph_a = ((const struct ufdt_phandle_table_entry *)pa)->phandle; |
| 274 | uint32_t ph_b = ((const struct ufdt_phandle_table_entry *)pb)->phandle; |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 275 | if (ph_a < ph_b) |
| 276 | return -1; |
| 277 | else if (ph_a == ph_b) |
| 278 | return 0; |
| 279 | else |
| 280 | return 1; |
| 281 | } |
| 282 | |
SzuWei Lin | 8a7039c | 2017-04-14 15:38:15 +0800 | [diff] [blame] | 283 | struct ufdt_static_phandle_table build_phandle_table(struct ufdt *tree) { |
| 284 | struct ufdt_static_phandle_table res; |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 285 | res.len = count_phandle_node(tree->root); |
SzuWei Lin | 8a7039c | 2017-04-14 15:38:15 +0800 | [diff] [blame] | 286 | res.data = dto_malloc(sizeof(struct ufdt_phandle_table_entry) * res.len); |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 287 | int cur = 0; |
| 288 | set_phandle_table_entry(tree->root, res.data, &cur); |
SzuWei Lin | 8a7039c | 2017-04-14 15:38:15 +0800 | [diff] [blame] | 289 | dto_qsort(res.data, res.len, sizeof(struct ufdt_phandle_table_entry), |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 290 | phandle_table_entry_cmp); |
| 291 | return res; |
| 292 | } |
| 293 | |
SzuWei Lin | d62a849 | 2017-04-24 10:17:15 +0800 | [diff] [blame] | 294 | struct ufdt *ufdt_from_fdt(void *fdtp, size_t fdt_size, |
| 295 | struct ufdt_node_pool *pool) { |
SzuWei Lin | 1be68ae | 2017-03-30 11:44:54 +0800 | [diff] [blame] | 296 | (void)(fdt_size); /* unused parameter */ |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 297 | |
| 298 | int start_offset = fdt_path_offset(fdtp, "/"); |
| 299 | if (start_offset < 0) { |
SzuWei Lin | d62a849 | 2017-04-24 10:17:15 +0800 | [diff] [blame] | 300 | return ufdt_construct(NULL, pool); |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 301 | } |
| 302 | |
| 303 | int end_offset; |
| 304 | int start_tag = fdt_next_tag(fdtp, start_offset, &end_offset); |
Mike McTernan | 14f8b87 | 2022-09-28 13:42:30 +0100 | [diff] [blame] | 305 | |
| 306 | if (start_tag != FDT_BEGIN_NODE) { |
| 307 | return ufdt_construct(NULL, pool); |
| 308 | } |
| 309 | |
| 310 | struct ufdt *res_tree = ufdt_construct(fdtp, pool); |
| 311 | if (res_tree == NULL) return NULL; |
| 312 | |
SzuWei Lin | d62a849 | 2017-04-24 10:17:15 +0800 | [diff] [blame] | 313 | res_tree->root = |
| 314 | fdt_to_ufdt_tree(fdtp, start_offset, &end_offset, start_tag, pool); |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 315 | |
| 316 | res_tree->phandle_table = build_phandle_table(res_tree); |
| 317 | |
| 318 | return res_tree; |
| 319 | } |
| 320 | |
SzuWei Lin | 1be68ae | 2017-03-30 11:44:54 +0800 | [diff] [blame] | 321 | static int _ufdt_get_property_nameoff(const struct ufdt *tree, const char *name, |
| 322 | const struct ufdt_prop_dict *dict) { |
| 323 | int res; |
| 324 | const struct fdt_property *same_name_prop = ufdt_prop_dict_find(dict, name); |
| 325 | if (same_name_prop != NULL) { |
| 326 | /* There is a property with same name, just use its string offset */ |
| 327 | res = fdt32_to_cpu(same_name_prop->nameoff); |
| 328 | } else { |
| 329 | /* Get the string offset from the string table of the current tree */ |
| 330 | res = ufdt_get_string_off(tree, name); |
| 331 | if (res == 0) { |
| 332 | dto_error("Cannot find property name in string table: %s\n", name); |
| 333 | return 0; |
| 334 | } |
| 335 | } |
| 336 | return res; |
| 337 | } |
| 338 | |
| 339 | static int _ufdt_output_property_to_fdt( |
| 340 | const struct ufdt *tree, void *fdtp, |
SzuWei Lin | 8a7039c | 2017-04-14 15:38:15 +0800 | [diff] [blame] | 341 | const struct ufdt_node_fdt_prop *prop_node, struct ufdt_prop_dict *dict) { |
SzuWei Lin | 1be68ae | 2017-03-30 11:44:54 +0800 | [diff] [blame] | 342 | int nameoff = _ufdt_get_property_nameoff(tree, prop_node->name, dict); |
| 343 | if (nameoff == 0) return -1; |
| 344 | |
| 345 | int data_len = 0; |
| 346 | void *data = ufdt_node_get_fdt_prop_data(&prop_node->parent, &data_len); |
Mike McTernan | cd47b8f | 2022-12-16 14:42:05 +0000 | [diff] [blame] | 347 | unsigned int aligned_data_len = |
| 348 | ((unsigned int)data_len + (FDT_TAGSIZE - 1u)) & ~(FDT_TAGSIZE - 1u); |
SzuWei Lin | 1be68ae | 2017-03-30 11:44:54 +0800 | [diff] [blame] | 349 | |
Mike McTernan | cd47b8f | 2022-12-16 14:42:05 +0000 | [diff] [blame] | 350 | unsigned int new_propoff = fdt_size_dt_struct(fdtp); |
| 351 | unsigned int new_prop_size = sizeof(struct fdt_property) + aligned_data_len; |
SzuWei Lin | 1be68ae | 2017-03-30 11:44:54 +0800 | [diff] [blame] | 352 | struct fdt_property *new_prop = |
| 353 | (struct fdt_property *)((char *)fdtp + fdt_off_dt_struct(fdtp) + |
| 354 | new_propoff); |
| 355 | char *fdt_end = (char *)fdtp + fdt_totalsize(fdtp); |
| 356 | if ((char *)new_prop + new_prop_size > fdt_end) { |
| 357 | dto_error("Not enough space for adding property.\n"); |
| 358 | return -1; |
| 359 | } |
| 360 | fdt_set_size_dt_struct(fdtp, new_propoff + new_prop_size); |
| 361 | |
| 362 | new_prop->tag = cpu_to_fdt32(FDT_PROP); |
| 363 | new_prop->nameoff = cpu_to_fdt32(nameoff); |
| 364 | new_prop->len = cpu_to_fdt32(data_len); |
| 365 | dto_memcpy(new_prop->data, data, data_len); |
| 366 | |
| 367 | ufdt_prop_dict_add(dict, new_prop); |
| 368 | |
| 369 | return 0; |
| 370 | } |
| 371 | |
| 372 | static int _ufdt_output_node_to_fdt(const struct ufdt *tree, void *fdtp, |
| 373 | const struct ufdt_node *node, |
| 374 | struct ufdt_prop_dict *dict) { |
SzuWei Lin | 8a7039c | 2017-04-14 15:38:15 +0800 | [diff] [blame] | 375 | uint32_t tag = ufdt_node_tag(node); |
SzuWei Lin | 1be68ae | 2017-03-30 11:44:54 +0800 | [diff] [blame] | 376 | |
| 377 | if (tag == FDT_PROP) { |
| 378 | return _ufdt_output_property_to_fdt( |
SzuWei Lin | 8a7039c | 2017-04-14 15:38:15 +0800 | [diff] [blame] | 379 | tree, fdtp, (const struct ufdt_node_fdt_prop *)node, dict); |
SzuWei Lin | 1be68ae | 2017-03-30 11:44:54 +0800 | [diff] [blame] | 380 | } |
| 381 | |
SzuWei Lin | 8a7039c | 2017-04-14 15:38:15 +0800 | [diff] [blame] | 382 | int err = fdt_begin_node(fdtp, ufdt_node_name(node)); |
SzuWei Lin | 1be68ae | 2017-03-30 11:44:54 +0800 | [diff] [blame] | 383 | if (err < 0) return -1; |
| 384 | |
| 385 | struct ufdt_node **it; |
| 386 | for_each_prop(it, node) { |
| 387 | err = _ufdt_output_node_to_fdt(tree, fdtp, *it, dict); |
| 388 | if (err < 0) return -1; |
| 389 | } |
| 390 | |
| 391 | for_each_node(it, node) { |
| 392 | err = _ufdt_output_node_to_fdt(tree, fdtp, *it, dict); |
| 393 | if (err < 0) return -1; |
| 394 | } |
| 395 | |
| 396 | err = fdt_end_node(fdtp); |
| 397 | if (err < 0) return -1; |
| 398 | |
| 399 | return 0; |
| 400 | } |
| 401 | |
| 402 | static int _ufdt_output_strtab_to_fdt(const struct ufdt *tree, void *fdt) { |
| 403 | /* Currently, we don't know the final dt_struct size, so we copy all |
| 404 | string tables to the end of the target fdt buffer in reversed order. |
| 405 | At last, fdt_finish() will adjust dt_string offset */ |
| 406 | const char *struct_top = |
| 407 | (char *)fdt + fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt); |
| 408 | char *dest = (char *)fdt + fdt_totalsize(fdt); |
| 409 | |
| 410 | int dest_size = 0; |
| 411 | for (int i = 0; i < tree->num_used_fdtps; i++) { |
| 412 | void *src_fdt = tree->fdtps[i]; |
| 413 | const char *src_strtab = (const char *)src_fdt + fdt_off_dt_strings(src_fdt); |
| 414 | int strtab_size = fdt_size_dt_strings(src_fdt); |
| 415 | |
| 416 | dest -= strtab_size; |
| 417 | if (dest < struct_top) { |
| 418 | dto_error("Not enough space for string table.\n"); |
| 419 | return -1; |
| 420 | } |
| 421 | |
| 422 | dto_memcpy(dest, src_strtab, strtab_size); |
| 423 | |
| 424 | dest_size += strtab_size; |
| 425 | } |
| 426 | |
| 427 | fdt_set_size_dt_strings(fdt, dest_size); |
| 428 | |
| 429 | return 0; |
| 430 | } |
| 431 | |
| 432 | int ufdt_to_fdt(const struct ufdt *tree, void *buf, int buf_size) { |
| 433 | if (tree->num_used_fdtps == 0) return -1; |
| 434 | |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 435 | int err; |
| 436 | err = fdt_create(buf, buf_size); |
| 437 | if (err < 0) return -1; |
| 438 | |
SzuWei Lin | 1be68ae | 2017-03-30 11:44:54 +0800 | [diff] [blame] | 439 | /* Here we output the memory reserve map of the ONLY FIRST fdt, |
| 440 | to be in compliance with the DTO behavior of libfdt. */ |
| 441 | int n_mem_rsv = fdt_num_mem_rsv(tree->fdtps[0]); |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 442 | for (int i = 0; i < n_mem_rsv; i++) { |
| 443 | uint64_t addr, size; |
SzuWei Lin | 1be68ae | 2017-03-30 11:44:54 +0800 | [diff] [blame] | 444 | fdt_get_mem_rsv(tree->fdtps[0], i, &addr, &size); |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 445 | fdt_add_reservemap_entry(buf, addr, size); |
| 446 | } |
| 447 | |
| 448 | err = fdt_finish_reservemap(buf); |
| 449 | if (err < 0) return -1; |
| 450 | |
SzuWei Lin | 1be68ae | 2017-03-30 11:44:54 +0800 | [diff] [blame] | 451 | err = _ufdt_output_strtab_to_fdt(tree, buf); |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 452 | if (err < 0) return -1; |
| 453 | |
SzuWei Lin | 1be68ae | 2017-03-30 11:44:54 +0800 | [diff] [blame] | 454 | struct ufdt_prop_dict dict; |
| 455 | err = ufdt_prop_dict_construct(&dict, buf); |
| 456 | if (err < 0) return -1; |
| 457 | |
| 458 | err = _ufdt_output_node_to_fdt(tree, buf, tree->root, &dict); |
| 459 | if (err < 0) return -1; |
| 460 | |
| 461 | ufdt_prop_dict_destruct(&dict); |
Li Chen | f6c209b | 2016-11-28 12:15:33 +0800 | [diff] [blame] | 462 | |
| 463 | err = fdt_finish(buf); |
| 464 | if (err < 0) return -1; |
| 465 | |
| 466 | /* |
| 467 | * IMPORTANT: fdt_totalsize(buf) might be less than buf_size |
| 468 | * so this is needed to make use of remain spaces. |
| 469 | */ |
| 470 | return fdt_open_into(buf, buf, buf_size); |
| 471 | } |