Adding upstream version 2.06.upstream/2.06upstream

Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>

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;
+}