diff options
Diffstat (limited to 'grub-core/lib/fdt.c')
| -rw-r--r-- | grub-core/lib/fdt.c | 531 |
1 files changed, 531 insertions, 0 deletions
diff --git a/grub-core/lib/fdt.c b/grub-core/lib/fdt.c new file mode 100644 index 0000000..0d371c5 --- /dev/null +++ b/grub-core/lib/fdt.c @@ -0,0 +1,531 @@ +/* + * GRUB -- GRand Unified Bootloader + * Copyright (C) 2013 Free Software Foundation, Inc. + * + * GRUB is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GRUB is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GRUB. If not, see <http://www.gnu.org/licenses/>. + */ + +#include <grub/fdt.h> +#include <grub/misc.h> +#include <grub/mm.h> +#include <grub/dl.h> + +GRUB_MOD_LICENSE ("GPLv3+"); + +#define FDT_SUPPORTED_VERSION 17 + +#define FDT_BEGIN_NODE 0x00000001 +#define FDT_END_NODE 0x00000002 +#define FDT_PROP 0x00000003 +#define FDT_NOP 0x00000004 +#define FDT_END 0x00000009 + +#define struct_end(fdt) \ + ((grub_addr_t) fdt + grub_fdt_get_off_dt_struct(fdt) \ + + grub_fdt_get_size_dt_struct(fdt)) + +/* Size needed by a node entry: 2 tokens (FDT_BEGIN_NODE and FDT_END_NODE), plus + the NULL-terminated string containing the name, plus padding if needed. */ +#define node_entry_size(node_name) \ + (2 * sizeof(grub_uint32_t) \ + + ALIGN_UP (grub_strlen (name) + 1, sizeof(grub_uint32_t))) + +#define SKIP_NODE_NAME(name, token, end) \ + name = (char *) ((token) + 1); \ + while (name < (char *) end) \ + { \ + if (!*name++) \ + break; \ + } \ + token = (grub_uint32_t *) ALIGN_UP((grub_addr_t) (name), sizeof(*token)) + + +static grub_uint32_t *get_next_node (const void *fdt, char *node_name) +{ + grub_uint32_t *end = (void *) struct_end (fdt); + grub_uint32_t *token; + + if (node_name >= (char *) end) + return NULL; + while (*node_name++) + { + if (node_name >= (char *) end) + return NULL; + } + token = (grub_uint32_t *) ALIGN_UP ((grub_addr_t) node_name, 4); + while (token < end) + { + switch (grub_be_to_cpu32(*token)) + { + case FDT_BEGIN_NODE: + token = get_next_node (fdt, (char *) (token + 1)); + if (!token) + return NULL; + break; + case FDT_END_NODE: + token++; + if (token >= end) + return NULL; + return token; + case FDT_PROP: + /* Skip property token and following data (len, nameoff and property + value). */ + token += grub_fdt_prop_entry_size(grub_be_to_cpu32(*(token + 1))) + / sizeof(*token); + break; + case FDT_NOP: + token++; + break; + default: + return NULL; + } + } + return NULL; +} + +static int get_mem_rsvmap_size (const void *fdt) +{ + int size = 0; + grub_unaligned_uint64_t *ptr = (void *) ((grub_addr_t) fdt + + grub_fdt_get_off_mem_rsvmap (fdt)); + + do + { + size += 2 * sizeof(*ptr); + if (!ptr[0].val && !ptr[1].val) + return size; + ptr += 2; + } while ((grub_addr_t) ptr <= (grub_addr_t) fdt + grub_fdt_get_totalsize (fdt) + - 2 * sizeof(grub_uint64_t)); + return -1; +} + +static grub_uint32_t get_free_space (void *fdt) +{ + int mem_rsvmap_size = get_mem_rsvmap_size (fdt); + + if (mem_rsvmap_size < 0) + /* invalid memory reservation block */ + return 0; + return (grub_fdt_get_totalsize (fdt) - sizeof(grub_fdt_header_t) + - mem_rsvmap_size - grub_fdt_get_size_dt_strings (fdt) + - grub_fdt_get_size_dt_struct (fdt)); +} + +static int add_subnode (void *fdt, int parentoffset, const char *name) +{ + grub_uint32_t *token = (void *) ((grub_addr_t) fdt + + grub_fdt_get_off_dt_struct(fdt) + + parentoffset); + grub_uint32_t *end = (void *) struct_end (fdt); + unsigned int entry_size = node_entry_size (name); + unsigned int struct_size = grub_fdt_get_size_dt_struct(fdt); + char *node_name; + + SKIP_NODE_NAME(node_name, token, end); + + /* Insert the new subnode just after the properties of the parent node (if + any).*/ + while (1) + { + if (token >= end) + return -1; + switch (grub_be_to_cpu32(*token)) + { + case FDT_PROP: + /* Skip len, nameoff and property value. */ + token += grub_fdt_prop_entry_size(grub_be_to_cpu32(*(token + 1))) + / sizeof(*token); + break; + case FDT_BEGIN_NODE: + case FDT_END_NODE: + goto insert; + case FDT_NOP: + token++; + break; + default: + /* invalid token */ + return -1; + } + } +insert: + grub_memmove (token + entry_size / sizeof(*token), token, + (grub_addr_t) end - (grub_addr_t) token); + *token = grub_cpu_to_be32_compile_time(FDT_BEGIN_NODE); + token[entry_size / sizeof(*token) - 2] = 0; /* padding bytes */ + grub_strcpy((char *) (token + 1), name); + token[entry_size / sizeof(*token) - 1] = grub_cpu_to_be32_compile_time(FDT_END_NODE); + grub_fdt_set_size_dt_struct (fdt, struct_size + entry_size); + return ((grub_addr_t) token - (grub_addr_t) fdt + - grub_fdt_get_off_dt_struct(fdt)); +} + +/* Rearrange FDT blocks in the canonical order: first the memory reservation + block (just after the FDT header), then the structure block and finally the + strings block. No free space is left between the first and the second block, + while the space between the second and the third block is given by the + clearance argument. */ +static int rearrange_blocks (void *fdt, unsigned int clearance) +{ + grub_uint32_t off_mem_rsvmap = ALIGN_UP(sizeof(grub_fdt_header_t), 8); + grub_uint32_t off_dt_struct = off_mem_rsvmap + get_mem_rsvmap_size (fdt); + grub_uint32_t off_dt_strings = off_dt_struct + + grub_fdt_get_size_dt_struct (fdt) + + clearance; + grub_uint8_t *fdt_ptr = fdt; + grub_uint8_t *tmp_fdt; + + if ((grub_fdt_get_off_mem_rsvmap (fdt) == off_mem_rsvmap) + && (grub_fdt_get_off_dt_struct (fdt) == off_dt_struct)) + { + /* No need to allocate memory for a temporary FDT, just move the strings + block if needed. */ + if (grub_fdt_get_off_dt_strings (fdt) != off_dt_strings) + { + grub_memmove(fdt_ptr + off_dt_strings, + fdt_ptr + grub_fdt_get_off_dt_strings (fdt), + grub_fdt_get_size_dt_strings (fdt)); + grub_fdt_set_off_dt_strings (fdt, off_dt_strings); + } + return 0; + } + tmp_fdt = grub_malloc (grub_fdt_get_totalsize (fdt)); + if (!tmp_fdt) + return -1; + grub_memcpy (tmp_fdt + off_mem_rsvmap, + fdt_ptr + grub_fdt_get_off_mem_rsvmap (fdt), + get_mem_rsvmap_size (fdt)); + grub_fdt_set_off_mem_rsvmap (fdt, off_mem_rsvmap); + grub_memcpy (tmp_fdt + off_dt_struct, + fdt_ptr + grub_fdt_get_off_dt_struct (fdt), + grub_fdt_get_size_dt_struct (fdt)); + grub_fdt_set_off_dt_struct (fdt, off_dt_struct); + grub_memcpy (tmp_fdt + off_dt_strings, + fdt_ptr + grub_fdt_get_off_dt_strings (fdt), + grub_fdt_get_size_dt_strings (fdt)); + grub_fdt_set_off_dt_strings (fdt, off_dt_strings); + + /* Copy reordered blocks back to fdt. */ + grub_memcpy (fdt_ptr + off_mem_rsvmap, tmp_fdt + off_mem_rsvmap, + grub_fdt_get_totalsize (fdt) - off_mem_rsvmap); + + grub_free(tmp_fdt); + return 0; +} + +static grub_uint32_t *find_prop (const void *fdt, unsigned int nodeoffset, + const char *name) +{ + grub_uint32_t *prop = (void *) ((grub_addr_t) fdt + + grub_fdt_get_off_dt_struct (fdt) + + nodeoffset); + grub_uint32_t *end = (void *) struct_end(fdt); + grub_uint32_t nameoff; + char *node_name; + + SKIP_NODE_NAME(node_name, prop, end); + while (prop < end - 2) + { + if (grub_be_to_cpu32(*prop) == FDT_PROP) + { + nameoff = grub_be_to_cpu32(*(prop + 2)); + if ((nameoff + grub_strlen (name) < grub_fdt_get_size_dt_strings (fdt)) + && !grub_strcmp (name, (char *) fdt + + grub_fdt_get_off_dt_strings (fdt) + nameoff)) + { + if (prop + grub_fdt_prop_entry_size(grub_be_to_cpu32(*(prop + 1))) + / sizeof (*prop) >= end) + return NULL; + return prop; + } + prop += grub_fdt_prop_entry_size(grub_be_to_cpu32(*(prop + 1))) / sizeof (*prop); + } + else if (grub_be_to_cpu32(*prop) == FDT_NOP) + prop++; + else + return NULL; + } + return NULL; +} + +/* Check the FDT header for consistency and adjust the totalsize field to match + the size allocated for the FDT; if this function is called before the other + functions in this file and returns success, the other functions are + guaranteed not to access memory locations outside the allocated memory. */ +int grub_fdt_check_header_nosize (const void *fdt) +{ + if (((grub_addr_t) fdt & 0x3) || (grub_fdt_get_magic (fdt) != FDT_MAGIC) + || (grub_fdt_get_version (fdt) < FDT_SUPPORTED_VERSION) + || (grub_fdt_get_last_comp_version (fdt) > FDT_SUPPORTED_VERSION) + || (grub_fdt_get_off_dt_struct (fdt) & 0x00000003) + || (grub_fdt_get_size_dt_struct (fdt) & 0x00000003) + || (grub_fdt_get_off_dt_struct (fdt) + grub_fdt_get_size_dt_struct (fdt) + > grub_fdt_get_totalsize (fdt)) + || (grub_fdt_get_off_dt_strings (fdt) + grub_fdt_get_size_dt_strings (fdt) + > grub_fdt_get_totalsize (fdt)) + || (grub_fdt_get_off_mem_rsvmap (fdt) & 0x00000007) + || (grub_fdt_get_off_mem_rsvmap (fdt) + > grub_fdt_get_totalsize (fdt) - 2 * sizeof(grub_uint64_t))) + return -1; + return 0; +} + +int grub_fdt_check_header (const void *fdt, unsigned int size) +{ + if (size < sizeof (grub_fdt_header_t) + || (grub_fdt_get_totalsize (fdt) > size) + || grub_fdt_check_header_nosize (fdt) == -1) + return -1; + return 0; +} + +static const grub_uint32_t * +advance_token (const void *fdt, const grub_uint32_t *token, const grub_uint32_t *end, int skip_current) +{ + for (; token < end; skip_current = 0) + { + switch (grub_be_to_cpu32 (*token)) + { + case FDT_BEGIN_NODE: + if (skip_current) + { + token = get_next_node (fdt, (char *) (token + 1)); + continue; + } + char *ptr; + for (ptr = (char *) (token + 1); *ptr && ptr < (char *) end; ptr++) + ; + if (ptr >= (char *) end) + return 0; + return token; + case FDT_PROP: + /* Skip property token and following data (len, nameoff and property + value). */ + if (token >= end - 1) + return 0; + token += grub_fdt_prop_entry_size(grub_be_to_cpu32(*(token + 1))) + / sizeof(*token); + break; + case FDT_NOP: + token++; + break; + default: + return 0; + } + } + return 0; +} + +int grub_fdt_next_node (const void *fdt, unsigned int currentoffset) +{ + const grub_uint32_t *token = (const grub_uint32_t *) fdt + (currentoffset + grub_fdt_get_off_dt_struct (fdt)) / 4; + token = advance_token (fdt, token, (const void *) struct_end (fdt), 1); + if (!token) + return -1; + return (int) ((grub_addr_t) token - (grub_addr_t) fdt + - grub_fdt_get_off_dt_struct (fdt)); +} + +int grub_fdt_first_node (const void *fdt, unsigned int parentoffset) +{ + const grub_uint32_t *token, *end; + char *node_name; + + if (parentoffset & 0x3) + return -1; + token = (const void *) ((grub_addr_t) fdt + grub_fdt_get_off_dt_struct(fdt) + + parentoffset); + end = (const void *) struct_end (fdt); + if ((token >= end) || (grub_be_to_cpu32(*token) != FDT_BEGIN_NODE)) + return -1; + SKIP_NODE_NAME(node_name, token, end); + token = advance_token (fdt, token, end, 0); + if (!token) + return -1; + return (int) ((grub_addr_t) token - (grub_addr_t) fdt + - grub_fdt_get_off_dt_struct (fdt)); +} + +/* Find a direct sub-node of a given parent node. */ +int grub_fdt_find_subnode (const void *fdt, unsigned int parentoffset, + const char *name) +{ + const grub_uint32_t *token, *end; + const char *node_name; + int skip_current = 0; + + if (parentoffset & 0x3) + return -1; + token = (const void *) ((grub_addr_t) fdt + grub_fdt_get_off_dt_struct(fdt) + + parentoffset); + end = (const void *) struct_end (fdt); + if ((token >= end) || (grub_be_to_cpu32(*token) != FDT_BEGIN_NODE)) + return -1; + SKIP_NODE_NAME(node_name, token, end); + while (1) { + token = advance_token (fdt, token, end, skip_current); + if (!token) + return -1; + skip_current = 1; + node_name = (const char *) token + 4; + if (grub_strcmp (node_name, name) == 0) + return (int) ((grub_addr_t) token - (grub_addr_t) fdt + - grub_fdt_get_off_dt_struct (fdt)); + } +} + +const char * +grub_fdt_get_nodename (const void *fdt, unsigned int nodeoffset) +{ + return (const char *) fdt + grub_fdt_get_off_dt_struct(fdt) + nodeoffset + 4; +} + +int grub_fdt_add_subnode (void *fdt, unsigned int parentoffset, + const char *name) +{ + unsigned int entry_size = node_entry_size(name); + + if ((parentoffset & 0x3) || (get_free_space (fdt) < entry_size)) + return -1; + + /* The new node entry will increase the size of the structure block: rearrange + blocks such that there is sufficient free space between the structure and + the strings block, then add the new node entry. */ + if (rearrange_blocks (fdt, entry_size) < 0) + return -1; + return add_subnode (fdt, parentoffset, name); +} + +const void * +grub_fdt_get_prop (const void *fdt, unsigned int nodeoffset, const char *name, + grub_uint32_t *len) +{ + grub_uint32_t *prop; + if ((nodeoffset >= grub_fdt_get_size_dt_struct (fdt)) || (nodeoffset & 0x3) + || (grub_be_to_cpu32(*(grub_uint32_t *) ((grub_addr_t) fdt + + grub_fdt_get_off_dt_struct (fdt) + nodeoffset)) + != FDT_BEGIN_NODE)) + return 0; + prop = find_prop (fdt, nodeoffset, name); + if (!prop) + return 0; + if (len) + *len = grub_be_to_cpu32 (*(prop + 1)); + return prop + 3; +} + +int grub_fdt_set_prop (void *fdt, unsigned int nodeoffset, const char *name, + const void *val, grub_uint32_t len) +{ + grub_uint32_t *prop; + int prop_name_present = 0; + grub_uint32_t nameoff = 0; + + if ((nodeoffset >= grub_fdt_get_size_dt_struct (fdt)) || (nodeoffset & 0x3) + || (grub_be_to_cpu32(*(grub_uint32_t *) ((grub_addr_t) fdt + + grub_fdt_get_off_dt_struct (fdt) + nodeoffset)) + != FDT_BEGIN_NODE)) + return -1; + prop = find_prop (fdt, nodeoffset, name); + if (prop) + { + grub_uint32_t prop_len = ALIGN_UP(grub_be_to_cpu32 (*(prop + 1)), + sizeof(grub_uint32_t)); + grub_uint32_t i; + + prop_name_present = 1; + for (i = 0; i < prop_len / sizeof(grub_uint32_t); i++) + *(prop + 3 + i) = grub_cpu_to_be32_compile_time (FDT_NOP); + if (len > ALIGN_UP(prop_len, sizeof(grub_uint32_t))) + { + /* Length of new property value is greater than the space allocated + for the current value: a new entry needs to be created, so save the + nameoff field of the current entry and replace the current entry + with NOP tokens. */ + nameoff = grub_be_to_cpu32 (*(prop + 2)); + *prop = *(prop + 1) = *(prop + 2) = grub_cpu_to_be32_compile_time (FDT_NOP); + prop = NULL; + } + } + if (!prop || !prop_name_present) { + unsigned int needed_space = 0; + + if (!prop) + needed_space = grub_fdt_prop_entry_size(len); + if (!prop_name_present) + needed_space += grub_strlen (name) + 1; + if (needed_space > get_free_space (fdt)) + return -1; + if (rearrange_blocks (fdt, !prop ? grub_fdt_prop_entry_size(len) : 0) < 0) + return -1; + } + if (!prop_name_present) { + /* Append the property name at the end of the strings block. */ + nameoff = grub_fdt_get_size_dt_strings (fdt); + grub_strcpy ((char *) fdt + grub_fdt_get_off_dt_strings (fdt) + nameoff, + name); + grub_fdt_set_size_dt_strings (fdt, grub_fdt_get_size_dt_strings (fdt) + + grub_strlen (name) + 1); + } + if (!prop) { + char *node_name = (char *) ((grub_addr_t) fdt + + grub_fdt_get_off_dt_struct (fdt) + nodeoffset + + sizeof(grub_uint32_t)); + + prop = (void *) (node_name + ALIGN_UP(grub_strlen(node_name) + 1, 4)); + grub_memmove (prop + grub_fdt_prop_entry_size(len) / sizeof(*prop), prop, + struct_end(fdt) - (grub_addr_t) prop); + grub_fdt_set_size_dt_struct (fdt, grub_fdt_get_size_dt_struct (fdt) + + grub_fdt_prop_entry_size(len)); + *prop = grub_cpu_to_be32_compile_time (FDT_PROP); + *(prop + 2) = grub_cpu_to_be32 (nameoff); + } + *(prop + 1) = grub_cpu_to_be32 (len); + + /* Insert padding bytes at the end of the value; if they are not needed, they + will be overwritten by the following memcpy. */ + *(prop + grub_fdt_prop_entry_size(len) / sizeof(grub_uint32_t) - 1) = 0; + + grub_memcpy (prop + 3, val, len); + return 0; +} + +int +grub_fdt_create_empty_tree (void *fdt, unsigned int size) +{ + struct grub_fdt_empty_tree *et; + + if (size < GRUB_FDT_EMPTY_TREE_SZ) + return -1; + + grub_memset (fdt, 0, size); + et = fdt; + + et->empty_node.tree_end = grub_cpu_to_be32_compile_time (FDT_END); + et->empty_node.node_end = grub_cpu_to_be32_compile_time (FDT_END_NODE); + et->empty_node.node_start = grub_cpu_to_be32_compile_time (FDT_BEGIN_NODE); + ((struct grub_fdt_empty_tree *) fdt)->header.off_mem_rsvmap = + grub_cpu_to_be32_compile_time (ALIGN_UP (sizeof (grub_fdt_header_t), 8)); + + grub_fdt_set_off_dt_strings (fdt, sizeof (*et)); + grub_fdt_set_off_dt_struct (fdt, + sizeof (et->header) + sizeof (et->empty_rsvmap)); + grub_fdt_set_version (fdt, FDT_SUPPORTED_VERSION); + grub_fdt_set_last_comp_version (fdt, FDT_SUPPORTED_VERSION); + grub_fdt_set_size_dt_struct (fdt, sizeof (et->empty_node)); + grub_fdt_set_totalsize (fdt, size); + grub_fdt_set_magic (fdt, FDT_MAGIC); + + return 0; +} |