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