lib/libufdt/ufdt_convert.c - kernel/lk - Gitiles

 #include "libufdt.h"

 #include "fdt_internal.h"
 #include "ufdt_util.h"


 struct ufdt *ufdt_construct(void *fdtp) {
   struct ufdt *res_ufdt = dto_malloc(sizeof(struct ufdt));
   res_ufdt->fdtp = fdtp;
   res_ufdt->root = NULL;

   return res_ufdt;
 }

 void ufdt_destruct(struct ufdt *tree) {
   ufdt_node_destruct(tree->root);
   dto_free(tree->phandle_table.data);
 }

 static struct ufdt_node *ufdt_new_node(void *fdtp, int node_offset) {
   if (fdtp == NULL) {
     dto_error("Failed to get new_node because tree is NULL\n");
     return NULL;
   }

   fdt32_t *fdt_tag_ptr =
       (fdt32_t *)fdt_offset_ptr(fdtp, node_offset, sizeof(fdt32_t));
   struct ufdt_node *res = ufdt_node_construct(fdtp, fdt_tag_ptr);
   return res;
 }

 static struct ufdt_node *fdt_to_ufdt_tree(void *fdtp, int cur_fdt_tag_offset,
                                           int *next_fdt_tag_offset,
                                           int cur_tag) {
   if (fdtp == NULL) {
     return NULL;
   }
   uint32_t tag;
   struct ufdt_node *res, *child_node;

   res = NULL;
   child_node = NULL;
   tag = cur_tag;

   switch (tag) {
     case FDT_END_NODE:
     case FDT_NOP:
     case FDT_END:
       break;

     case FDT_PROP:
       res = ufdt_new_node(fdtp, cur_fdt_tag_offset);
       break;

     case FDT_BEGIN_NODE:
       res = ufdt_new_node(fdtp, cur_fdt_tag_offset);

       do {
         cur_fdt_tag_offset = *next_fdt_tag_offset;
         tag = fdt_next_tag(fdtp, cur_fdt_tag_offset, next_fdt_tag_offset);
         child_node = fdt_to_ufdt_tree(fdtp, cur_fdt_tag_offset,
                                       next_fdt_tag_offset, tag);
         ufdt_node_add_child(res, child_node);
       } while (tag != FDT_END_NODE);
       break;

     default:
       break;
   }

   return res;
 }

 void ufdt_print(struct ufdt *tree) { ufdt_node_print(tree->root, 0); }

 struct ufdt_node *ufdt_get_node_by_path_len(struct ufdt *tree, const char *path,
                                             int len) {
   /*
    * RARE: aliases
    * In device tree, we can assign some alias to specific nodes by defining
    * these relation in "/aliases" node.
    * The node has the form:
    * {
    *   a = "/a_for_apple";
    *   b = "/b_for_banana";
    * };
    * So the path "a/subnode_1" should be expanded to "/a_for_apple/subnode_1".
    */
   if (*path != '/') {
     const char *end = path + len;

     const char *next_slash;
     next_slash = dto_memchr(path, '/', end - path);
     if (!next_slash) next_slash = end;

     struct ufdt_node *aliases_node =
         ufdt_node_get_node_by_path(tree->root, "/aliases");
     aliases_node = ufdt_node_get_property_by_name_len(aliases_node, path,
                                                       next_slash - path);

     int path_len = 0;
     const char *alias_path =
         ufdt_node_get_fdt_prop_data(aliases_node, &path_len);

     if (alias_path == NULL) {
       dto_error("Failed to find alias %s\n", path);
       return NULL;
     }

     struct ufdt_node *target_node =
         ufdt_node_get_node_by_path_len(tree->root, alias_path, path_len);

     return ufdt_node_get_node_by_path_len(target_node, next_slash,
                                           end - next_slash);
   }
   return ufdt_node_get_node_by_path_len(tree->root, path, len);
 }

 struct ufdt_node *ufdt_get_node_by_path(struct ufdt *tree, const char *path) {
   return ufdt_get_node_by_path_len(tree, path, dto_strlen(path));
 }

 struct ufdt_node *ufdt_get_node_by_phandle(struct ufdt *tree,
                                            uint32_t phandle) {
   struct ufdt_node *res = NULL;
   /*
    * Do binary search in phandle_table.data.
    * [s, e) means the possible range which contains target node.
    */
   int s = 0, e = tree->phandle_table.len;
   while (e - s > 1) {
     int mid = s + ((e - s) >> 1);
     uint32_t mid_phandle = tree->phandle_table.data[mid].phandle;
     if (phandle < mid_phandle)
       e = mid;
     else
       s = mid;
   }
   if (e - s > 0) {
     res = tree->phandle_table.data[s].node;
   }
   return res;
 }

 int merge_children(struct ufdt_node *node_a, struct ufdt_node *node_b) {
   int err = 0;
   struct ufdt_node *it;
   for (it = ((struct fdt_node_ufdt_node *)node_b)->child; it;) {
     struct ufdt_node *cur_node = it;
     it = it->sibling;
     cur_node->sibling = NULL;
     struct ufdt_node *target_node = NULL;
     if (tag_of(cur_node) == FDT_BEGIN_NODE) {
       target_node = ufdt_node_get_subnode_by_name(node_a, name_of(cur_node));
     } else {
       target_node = ufdt_node_get_property_by_name(node_a, name_of(cur_node));
     }
     if (target_node == NULL) {
       err = ufdt_node_add_child(node_a, cur_node);
     } else {
       err = merge_ufdt_into(target_node, cur_node);
     }
     if (err < 0) return -1;
   }
   /*
    * The ufdt_node* in node_b will be copied to node_a.
    * To prevent the ufdt_node from being freed twice
    * (main_tree and overlay_tree) at the end of function
    * ufdt_apply_overlay(), set this node in node_b
    * (overlay_tree) to NULL.
    */
   ((struct fdt_node_ufdt_node *)node_b)->child = NULL;

   return 0;
 }

 int merge_ufdt_into(struct ufdt_node *node_a, struct ufdt_node *node_b) {
   if (tag_of(node_a) == FDT_PROP) {
     node_a->fdt_tag_ptr = node_b->fdt_tag_ptr;
     return 0;
   }

   int err = 0;
   err = merge_children(node_a, node_b);
   if (err < 0) return -1;

   return 0;
 }

 void ufdt_map(struct ufdt *tree, struct ufdt_node_closure closure) {
   ufdt_node_map(tree->root, closure);
 }

 static int count_phandle_node(struct ufdt_node *node) {
   if (node == NULL) return 0;
   if (tag_of(node) != FDT_BEGIN_NODE) return 0;
   int res = 0;
   if (ufdt_node_get_phandle(node) > 0) res++;
   struct ufdt_node **it;
   for_each_child(it, node) { res += count_phandle_node(*it); }
   return res;
 }

 static void set_phandle_table_entry(struct ufdt_node *node,
                                     struct phandle_table_entry *data,
                                     int *cur) {
   if (node == NULL || tag_of(node) != FDT_BEGIN_NODE) return;
   int ph = ufdt_node_get_phandle(node);
   if (ph > 0) {
     data[*cur].phandle = ph;
     data[*cur].node = node;
     (*cur)++;
   }
   struct ufdt_node **it;
   for_each_node(it, node) set_phandle_table_entry(*it, data, cur);
   return;
 }

 int phandle_table_entry_cmp(const void *pa, const void *pb) {
   uint32_t ph_a = ((const struct phandle_table_entry *)pa)->phandle;
   uint32_t ph_b = ((const struct phandle_table_entry *)pb)->phandle;
   if (ph_a < ph_b)
     return -1;
   else if (ph_a == ph_b)
     return 0;
   else
     return 1;
 }

 struct static_phandle_table build_phandle_table(struct ufdt *tree) {
   struct static_phandle_table res;
   res.len = count_phandle_node(tree->root);
   res.data = dto_malloc(sizeof(struct phandle_table_entry) * res.len);
   int cur = 0;
   set_phandle_table_entry(tree->root, res.data, &cur);
   dto_qsort(res.data, res.len, sizeof(struct phandle_table_entry),
             phandle_table_entry_cmp);
   return res;
 }

 struct ufdt *fdt_to_ufdt(void *fdtp, size_t fdt_size) {
   (void)(fdt_size); // unused parameter

   struct ufdt *res_tree = ufdt_construct(fdtp);

   int start_offset = fdt_path_offset(fdtp, "/");
   if (start_offset < 0) {
     res_tree->fdtp = NULL;
     return res_tree;
   }

   int end_offset;
   int start_tag = fdt_next_tag(fdtp, start_offset, &end_offset);
   res_tree->root = fdt_to_ufdt_tree(fdtp, start_offset, &end_offset, start_tag);

   res_tree->phandle_table = build_phandle_table(res_tree);

   return res_tree;
 }

 int ufdt_to_fdt(struct ufdt *tree, void *buf, int buf_size) {
   int err;
   err = fdt_create(buf, buf_size);
   if (err < 0) return -1;

   int n_mem_rsv = fdt_num_mem_rsv(tree->fdtp);
   for (int i = 0; i < n_mem_rsv; i++) {
     uint64_t addr, size;
     fdt_get_mem_rsv(tree->fdtp, i, &addr, &size);
     fdt_add_reservemap_entry(buf, addr, size);
   }

   err = fdt_finish_reservemap(buf);
   if (err < 0) return -1;

   /*
    * Obtains all props for later use because getting them from
    * FDT requires complicated manipulation.
   */
   struct ufdt_node_dict all_props = ufdt_node_dict_construct();
   err = output_ufdt_node_to_fdt(tree->root, buf, &all_props);
   if (err < 0) return -1;

   ufdt_node_dict_destruct(&all_props);

   err = fdt_finish(buf);
   if (err < 0) return -1;

   /*
    * IMPORTANT: fdt_totalsize(buf) might be less than buf_size
    * so this is needed to make use of remain spaces.
    */
   return fdt_open_into(buf, buf, buf_size);
 }
	#include "libufdt.h"

	#include "fdt_internal.h"
	#include "ufdt_util.h"


	struct ufdt ufdt_construct(void fdtp) {
	struct ufdt *res_ufdt = dto_malloc(sizeof(struct ufdt));
	res_ufdt->fdtp = fdtp;
	res_ufdt->root = NULL;

	return res_ufdt;
	}

	void ufdt_destruct(struct ufdt *tree) {
	ufdt_node_destruct(tree->root);
	dto_free(tree->phandle_table.data);
	}

	static struct ufdt_node ufdt_new_node(void fdtp, int node_offset) {
	if (fdtp == NULL) {
	dto_error("Failed to get new_node because tree is NULL\n");
	return NULL;
	}

	fdt32_t *fdt_tag_ptr =
	(fdt32_t *)fdt_offset_ptr(fdtp, node_offset, sizeof(fdt32_t));
	struct ufdt_node *res = ufdt_node_construct(fdtp, fdt_tag_ptr);
	return res;
	}

	static struct ufdt_node fdt_to_ufdt_tree(void fdtp, int cur_fdt_tag_offset,
	int *next_fdt_tag_offset,
	int cur_tag) {
	if (fdtp == NULL) {
	return NULL;
	}
	uint32_t tag;
	struct ufdt_node res, child_node;

	res = NULL;
	child_node = NULL;
	tag = cur_tag;

	switch (tag) {
	case FDT_END_NODE:
	case FDT_NOP:
	case FDT_END:
	break;

	case FDT_PROP:
	res = ufdt_new_node(fdtp, cur_fdt_tag_offset);
	break;

	case FDT_BEGIN_NODE:
	res = ufdt_new_node(fdtp, cur_fdt_tag_offset);

	do {
	cur_fdt_tag_offset = *next_fdt_tag_offset;
	tag = fdt_next_tag(fdtp, cur_fdt_tag_offset, next_fdt_tag_offset);
	child_node = fdt_to_ufdt_tree(fdtp, cur_fdt_tag_offset,
	next_fdt_tag_offset, tag);
	ufdt_node_add_child(res, child_node);
	} while (tag != FDT_END_NODE);
	break;

	default:
	break;
	}

	return res;
	}

	void ufdt_print(struct ufdt *tree) { ufdt_node_print(tree->root, 0); }

	struct ufdt_node ufdt_get_node_by_path_len(struct ufdt tree, const char *path,
	int len) {
	/*
	* RARE: aliases
	* In device tree, we can assign some alias to specific nodes by defining
	* these relation in "/aliases" node.
	* The node has the form:
	* {
	* a = "/a_for_apple";
	* b = "/b_for_banana";
	* };
	* So the path "a/subnode_1" should be expanded to "/a_for_apple/subnode_1".
	*/
	if (*path != '/') {
	const char *end = path + len;

	const char *next_slash;
	next_slash = dto_memchr(path, '/', end - path);
	if (!next_slash) next_slash = end;

	struct ufdt_node *aliases_node =
	ufdt_node_get_node_by_path(tree->root, "/aliases");
	aliases_node = ufdt_node_get_property_by_name_len(aliases_node, path,
	next_slash - path);

	int path_len = 0;
	const char *alias_path =
	ufdt_node_get_fdt_prop_data(aliases_node, &path_len);

	if (alias_path == NULL) {
	dto_error("Failed to find alias %s\n", path);
	return NULL;
	}

	struct ufdt_node *target_node =
	ufdt_node_get_node_by_path_len(tree->root, alias_path, path_len);

	return ufdt_node_get_node_by_path_len(target_node, next_slash,
	end - next_slash);
	}
	return ufdt_node_get_node_by_path_len(tree->root, path, len);
	}

	struct ufdt_node ufdt_get_node_by_path(struct ufdt tree, const char *path) {
	return ufdt_get_node_by_path_len(tree, path, dto_strlen(path));
	}

	struct ufdt_node ufdt_get_node_by_phandle(struct ufdt tree,
	uint32_t phandle) {
	struct ufdt_node *res = NULL;
	/*
	* Do binary search in phandle_table.data.
	* [s, e) means the possible range which contains target node.
	*/
	int s = 0, e = tree->phandle_table.len;
	while (e - s > 1) {
	int mid = s + ((e - s) >> 1);
	uint32_t mid_phandle = tree->phandle_table.data[mid].phandle;
	if (phandle < mid_phandle)
	e = mid;
	else
	s = mid;
	}
	if (e - s > 0) {
	res = tree->phandle_table.data[s].node;
	}
	return res;
	}

	int merge_children(struct ufdt_node node_a, struct ufdt_node node_b) {
	int err = 0;
	struct ufdt_node *it;
	for (it = ((struct fdt_node_ufdt_node *)node_b)->child; it;) {
	struct ufdt_node *cur_node = it;
	it = it->sibling;
	cur_node->sibling = NULL;
	struct ufdt_node *target_node = NULL;
	if (tag_of(cur_node) == FDT_BEGIN_NODE) {
	target_node = ufdt_node_get_subnode_by_name(node_a, name_of(cur_node));
	} else {
	target_node = ufdt_node_get_property_by_name(node_a, name_of(cur_node));
	}
	if (target_node == NULL) {
	err = ufdt_node_add_child(node_a, cur_node);
	} else {
	err = merge_ufdt_into(target_node, cur_node);
	}
	if (err < 0) return -1;
	}
	/*
	* The ufdt_node* in node_b will be copied to node_a.
	* To prevent the ufdt_node from being freed twice
	* (main_tree and overlay_tree) at the end of function
	* ufdt_apply_overlay(), set this node in node_b
	* (overlay_tree) to NULL.
	*/
	((struct fdt_node_ufdt_node *)node_b)->child = NULL;

	return 0;
	}

	int merge_ufdt_into(struct ufdt_node node_a, struct ufdt_node node_b) {
	if (tag_of(node_a) == FDT_PROP) {
	node_a->fdt_tag_ptr = node_b->fdt_tag_ptr;
	return 0;
	}

	int err = 0;
	err = merge_children(node_a, node_b);
	if (err < 0) return -1;

	return 0;
	}

	void ufdt_map(struct ufdt *tree, struct ufdt_node_closure closure) {
	ufdt_node_map(tree->root, closure);
	}

	static int count_phandle_node(struct ufdt_node *node) {
	if (node == NULL) return 0;
	if (tag_of(node) != FDT_BEGIN_NODE) return 0;
	int res = 0;
	if (ufdt_node_get_phandle(node) > 0) res++;
	struct ufdt_node **it;
	for_each_child(it, node) { res += count_phandle_node(*it); }
	return res;
	}

	static void set_phandle_table_entry(struct ufdt_node *node,
	struct phandle_table_entry *data,
	int *cur) {
	if (node == NULL \|\| tag_of(node) != FDT_BEGIN_NODE) return;
	int ph = ufdt_node_get_phandle(node);
	if (ph > 0) {
	data[*cur].phandle = ph;
	data[*cur].node = node;
	(*cur)++;
	}
	struct ufdt_node **it;
	for_each_node(it, node) set_phandle_table_entry(*it, data, cur);
	return;
	}

	int phandle_table_entry_cmp(const void pa, const void pb) {
	uint32_t ph_a = ((const struct phandle_table_entry *)pa)->phandle;
	uint32_t ph_b = ((const struct phandle_table_entry *)pb)->phandle;
	if (ph_a < ph_b)
	return -1;
	else if (ph_a == ph_b)
	return 0;
	else
	return 1;
	}

	struct static_phandle_table build_phandle_table(struct ufdt *tree) {
	struct static_phandle_table res;
	res.len = count_phandle_node(tree->root);
	res.data = dto_malloc(sizeof(struct phandle_table_entry) * res.len);
	int cur = 0;
	set_phandle_table_entry(tree->root, res.data, &cur);
	dto_qsort(res.data, res.len, sizeof(struct phandle_table_entry),
	phandle_table_entry_cmp);
	return res;
	}

	struct ufdt fdt_to_ufdt(void fdtp, size_t fdt_size) {
	(void)(fdt_size); // unused parameter

	struct ufdt *res_tree = ufdt_construct(fdtp);

	int start_offset = fdt_path_offset(fdtp, "/");
	if (start_offset < 0) {
	res_tree->fdtp = NULL;
	return res_tree;
	}

	int end_offset;
	int start_tag = fdt_next_tag(fdtp, start_offset, &end_offset);
	res_tree->root = fdt_to_ufdt_tree(fdtp, start_offset, &end_offset, start_tag);

	res_tree->phandle_table = build_phandle_table(res_tree);

	return res_tree;
	}

	int ufdt_to_fdt(struct ufdt tree, void buf, int buf_size) {
	int err;
	err = fdt_create(buf, buf_size);
	if (err < 0) return -1;

	int n_mem_rsv = fdt_num_mem_rsv(tree->fdtp);
	for (int i = 0; i < n_mem_rsv; i++) {
	uint64_t addr, size;
	fdt_get_mem_rsv(tree->fdtp, i, &addr, &size);
	fdt_add_reservemap_entry(buf, addr, size);
	}

	err = fdt_finish_reservemap(buf);
	if (err < 0) return -1;

	/*
	* Obtains all props for later use because getting them from
	* FDT requires complicated manipulation.
	*/
	struct ufdt_node_dict all_props = ufdt_node_dict_construct();
	err = output_ufdt_node_to_fdt(tree->root, buf, &all_props);
	if (err < 0) return -1;

	ufdt_node_dict_destruct(&all_props);

	err = fdt_finish(buf);
	if (err < 0) return -1;

	/*
	* IMPORTANT: fdt_totalsize(buf) might be less than buf_size
	* so this is needed to make use of remain spaces.
	*/
	return fdt_open_into(buf, buf, buf_size);
	}