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
22struct 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
46error:
47  if (res_ufdt) dto_free(res_ufdt);
48  if (fdtps) dto_free(fdtps);
49
50  return NULL;
51}
52
53void 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
63int 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
87int 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
111static 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
124static 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
166void ufdt_print(struct ufdt *tree) {
167  ufdt_node_print(tree->root, 0);
168}
169
170struct 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
213struct 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
217struct 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
239static 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
249static 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
264int 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
275struct 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
286struct 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
306static 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
324static 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
356static 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
386static 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
416int 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