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 
ufdt_construct(void * fdtp,struct ufdt_node_pool * pool)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 
ufdt_destruct(struct ufdt * tree,struct ufdt_node_pool * pool)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 
ufdt_add_fdt(struct ufdt * tree,void * fdtp)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 
ufdt_get_string_off(const struct ufdt * tree,const char * s)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 
ufdt_new_node(void * fdtp,int node_offset,struct ufdt_node_pool * pool)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 
fdt_to_ufdt_tree(void * fdtp,int cur_fdt_tag_offset,int * next_fdt_tag_offset,int cur_tag,struct ufdt_node_pool * pool)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 
ufdt_print(struct ufdt * tree)166 void ufdt_print(struct ufdt *tree) {
167   ufdt_node_print(tree->root, 0);
168 }
169 
ufdt_get_node_by_path_len(struct ufdt * tree,const char * path,int len)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 
ufdt_get_node_by_path(struct ufdt * tree,const char * path)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 
ufdt_get_node_by_phandle(struct ufdt * tree,uint32_t phandle)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 
count_phandle_node(struct ufdt_node * node)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 
set_phandle_table_entry(struct ufdt_node * node,struct ufdt_phandle_table_entry * data,int * cur)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 
phandle_table_entry_cmp(const void * pa,const void * pb)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 
build_phandle_table(struct ufdt * tree)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 
ufdt_from_fdt(void * fdtp,size_t fdt_size,struct ufdt_node_pool * pool)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 
_ufdt_get_property_nameoff(const struct ufdt * tree,const char * name,const struct ufdt_prop_dict * dict)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 
_ufdt_output_property_to_fdt(const struct ufdt * tree,void * fdtp,const struct ufdt_node_fdt_prop * prop_node,struct ufdt_prop_dict * dict)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 
_ufdt_output_node_to_fdt(const struct ufdt * tree,void * fdtp,const struct ufdt_node * node,struct ufdt_prop_dict * dict)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 
_ufdt_output_strtab_to_fdt(const struct ufdt * tree,void * fdt)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 
ufdt_to_fdt(const struct ufdt * tree,void * buf,int buf_size)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