grub-core/lib/fdt.c

   1 /*
   2  *  GRUB  --  GRand Unified Bootloader
   3  *  Copyright (C) 2013  Free Software Foundation, Inc.
   4  *
   5  *  GRUB is free software: you can redistribute it and/or modify
   6  *  it under the terms of the GNU General Public License as published by
   7  *  the Free Software Foundation, either version 3 of the License, or
   8  *  (at your option) any later version.
   9  *
  10  *  GRUB is distributed in the hope that it will be useful,
  11  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  *  GNU General Public License for more details.
  14  *
  15  *  You should have received a copy of the GNU General Public License
  16  *  along with GRUB.  If not, see <http://www.gnu.org/licenses/>.
  17  */
  18
  19 #include <grub/fdt.h>
  20 #include <grub/misc.h>
  21 #include <grub/mm.h>
  22 #include <grub/dl.h>
  23
  24 GRUB_MOD_LICENSE ("GPLv3+");
  25
  26 #define FDT_SUPPORTED_VERSION   17
  27
  28 #define FDT_BEGIN_NODE  0x00000001
  29 #define FDT_END_NODE    0x00000002
  30 #define FDT_PROP        0x00000003
  31 #define FDT_NOP         0x00000004
  32 #define FDT_END         0x00000009
  33
  34 #define struct_end(fdt) \
  35         ((grub_addr_t) fdt + grub_fdt_get_off_dt_struct(fdt)    \
  36          + grub_fdt_get_size_dt_struct(fdt))
  37
  38 /* Size needed by a node entry: 2 tokens (FDT_BEGIN_NODE and FDT_END_NODE), plus
  39    the NULL-terminated string containing the name, plus padding if needed. */
  40 #define node_entry_size(node_name)      \
  41         (2 * sizeof(grub_uint32_t)      \
  42         + ALIGN_UP (grub_strlen (name) + 1, sizeof(grub_uint32_t)))
  43
  44 /* Size needed by a property entry: 1 token (FDT_PROPERTY), plus len and nameoff
  45    fields, plus the property value, plus padding if needed. */
  46 #define prop_entry_size(prop_len)       \
  47         (3 * sizeof(grub_uint32_t) + ALIGN_UP(prop_len, sizeof(grub_uint32_t)))
  48
  49 #define SKIP_NODE_NAME(name, token, end)        \
  50   name = (char *) ((token) + 1);        \
  51   while (name < (char *) end)   \
  52   {     \
  53     if (!*name++)       \
  54       break;    \
  55   }     \
  56   token = (grub_uint32_t *) ALIGN_UP((grub_addr_t) (name), sizeof(*token))
  57
  58
  59 static grub_uint32_t *get_next_node (const void *fdt, char *node_name)
  60 {
  61   grub_uint32_t *end = (void *) struct_end (fdt);
  62   grub_uint32_t *token;
  63
  64   if (node_name >= (char *) end)
  65     return NULL;
  66   while (*node_name++)
  67   {
  68     if (node_name >= (char *) end)
  69           return NULL;
  70   }
  71   token = (grub_uint32_t *) ALIGN_UP ((grub_addr_t) node_name, 4);
  72   while (token < end)
  73   {
  74     switch (grub_be_to_cpu32(*token))
  75     {
  76       case FDT_BEGIN_NODE:
  77         token = get_next_node (fdt, (char *) (token + 1));
  78         if (!token)
  79           return NULL;
  80         break;
  81       case FDT_END_NODE:
  82         token++;
  83         if (token >= end)
  84           return NULL;
  85         return token;
  86       case FDT_PROP:
  87         /* Skip property token and following data (len, nameoff and property
  88            value). */
  89         token += prop_entry_size(grub_be_to_cpu32(*(token + 1)))
  90                  / sizeof(*token);
  91         break;
  92       case FDT_NOP:
  93         token++;
  94         break;
  95       default:
  96         return NULL;
  97     }
  98   }
  99   return NULL;
 100 }
 101
 102 static int get_mem_rsvmap_size (const void *fdt)
 103 {
 104   int size = 0;
 105   grub_unaligned_uint64_t *ptr = (void *) ((grub_addr_t) fdt
 106                                            + grub_fdt_get_off_mem_rsvmap (fdt));
 107
 108   do
 109   {
 110     size += 2 * sizeof(*ptr);
 111     if (!ptr[0].val && !ptr[1].val)
 112       return size;
 113     ptr += 2;
 114   } while ((grub_addr_t) ptr <= (grub_addr_t) fdt + grub_fdt_get_totalsize (fdt)
 115                                   - 2 * sizeof(grub_uint64_t));
 116   return -1;
 117 }
 118
 119 static grub_uint32_t get_free_space (void *fdt)
 120 {
 121   int mem_rsvmap_size = get_mem_rsvmap_size (fdt);
 122
 123   if (mem_rsvmap_size < 0)
 124     /* invalid memory reservation block */
 125     return 0;
 126   return (grub_fdt_get_totalsize (fdt) - sizeof(grub_fdt_header_t)
 127           - mem_rsvmap_size - grub_fdt_get_size_dt_strings (fdt)
 128           - grub_fdt_get_size_dt_struct (fdt));
 129 }
 130
 131 static int add_subnode (void *fdt, int parentoffset, const char *name)
 132 {
 133   grub_uint32_t *token = (void *) ((grub_addr_t) fdt
 134                                    + grub_fdt_get_off_dt_struct(fdt)
 135                                    + parentoffset);
 136   grub_uint32_t *end = (void *) struct_end (fdt);
 137   unsigned int entry_size = node_entry_size (name);
 138   unsigned int struct_size = grub_fdt_get_size_dt_struct(fdt);
 139   char *node_name;
 140
 141   SKIP_NODE_NAME(node_name, token, end);
 142
 143   /* Insert the new subnode just after the properties of the parent node (if
 144      any).*/
 145   while (1)
 146   {
 147     if (token >= end)
 148       return -1;
 149     switch (grub_be_to_cpu32(*token))
 150     {
 151       case FDT_PROP:
 152         /* Skip len, nameoff and property value. */
 153         token += prop_entry_size(grub_be_to_cpu32(*(token + 1)))
 154                  / sizeof(*token);
 155         break;
 156       case FDT_BEGIN_NODE:
 157       case FDT_END_NODE:
 158         goto insert;
 159       case FDT_NOP:
 160         token++;
 161         break;
 162       default:
 163         /* invalid token */
 164         return -1;
 165     }
 166   }
 167 insert:
 168   grub_memmove (token + entry_size / sizeof(*token), token,
 169                 (grub_addr_t) end - (grub_addr_t) token);
 170   *token = grub_cpu_to_be32_compile_time(FDT_BEGIN_NODE);
 171   token[entry_size / sizeof(*token) - 2] = 0;   /* padding bytes */
 172   grub_strcpy((char *) (token + 1), name);
 173   token[entry_size / sizeof(*token) - 1] = grub_cpu_to_be32_compile_time(FDT_END_NODE);
 174   grub_fdt_set_size_dt_struct (fdt, struct_size + entry_size);
 175   return ((grub_addr_t) token - (grub_addr_t) fdt
 176           - grub_fdt_get_off_dt_struct(fdt));
 177 }
 178
 179 /* Rearrange FDT blocks in the canonical order: first the memory reservation
 180    block (just after the FDT header), then the structure block and finally the
 181    strings block. No free space is left between the first and the second block,
 182    while the space between the second and the third block is given by the
 183    clearance argument. */
 184 static int rearrange_blocks (void *fdt, unsigned int clearance)
 185 {
 186   grub_uint32_t off_mem_rsvmap = ALIGN_UP(sizeof(grub_fdt_header_t), 8);
 187   grub_uint32_t off_dt_struct = off_mem_rsvmap + get_mem_rsvmap_size (fdt);
 188   grub_uint32_t off_dt_strings = off_dt_struct
 189                                  + grub_fdt_get_size_dt_struct (fdt)
 190                                  + clearance;
 191   grub_uint8_t *fdt_ptr = fdt;
 192   grub_uint8_t *tmp_fdt;
 193
 194   if ((grub_fdt_get_off_mem_rsvmap (fdt) == off_mem_rsvmap)
 195       && (grub_fdt_get_off_dt_struct (fdt) == off_dt_struct))
 196     {
 197       /* No need to allocate memory for a temporary FDT, just move the strings
 198          block if needed. */
 199       if (grub_fdt_get_off_dt_strings (fdt) != off_dt_strings)
 200         {
 201           grub_memmove(fdt_ptr + off_dt_strings,
 202                        fdt_ptr + grub_fdt_get_off_dt_strings (fdt),
 203                        grub_fdt_get_size_dt_strings (fdt));
 204           grub_fdt_set_off_dt_strings (fdt, off_dt_strings);
 205         }
 206       return 0;
 207     }
 208   tmp_fdt = grub_malloc (grub_fdt_get_totalsize (fdt));
 209   if (!tmp_fdt)
 210     return -1;
 211   grub_memcpy (tmp_fdt + off_mem_rsvmap,
 212                fdt_ptr + grub_fdt_get_off_mem_rsvmap (fdt),
 213                get_mem_rsvmap_size (fdt));
 214   grub_fdt_set_off_mem_rsvmap (fdt, off_mem_rsvmap);
 215   grub_memcpy (tmp_fdt + off_dt_struct,
 216                fdt_ptr + grub_fdt_get_off_dt_struct (fdt),
 217                grub_fdt_get_size_dt_struct (fdt));
 218   grub_fdt_set_off_dt_struct (fdt, off_dt_struct);
 219   grub_memcpy (tmp_fdt + off_dt_strings,
 220                fdt_ptr + grub_fdt_get_off_dt_strings (fdt),
 221                grub_fdt_get_size_dt_strings (fdt));
 222   grub_fdt_set_off_dt_strings (fdt, off_dt_strings);
 223
 224   /* Copy reordered blocks back to fdt. */
 225   grub_memcpy (fdt_ptr + off_mem_rsvmap, tmp_fdt + off_mem_rsvmap,
 226                grub_fdt_get_totalsize (fdt) - off_mem_rsvmap);
 227
 228   grub_free(tmp_fdt);
 229   return 0;
 230 }
 231
 232 static grub_uint32_t *find_prop (const void *fdt, unsigned int nodeoffset,
 233                                  const char *name)
 234 {
 235   grub_uint32_t *prop = (void *) ((grub_addr_t) fdt
 236                                  + grub_fdt_get_off_dt_struct (fdt)
 237                                  + nodeoffset);
 238   grub_uint32_t *end = (void *) struct_end(fdt);
 239   grub_uint32_t nameoff;
 240   char *node_name;
 241
 242   SKIP_NODE_NAME(node_name, prop, end);
 243   while (prop < end - 2)
 244   {
 245     if (grub_be_to_cpu32(*prop) == FDT_PROP)
 246       {
 247         nameoff = grub_be_to_cpu32(*(prop + 2));
 248         if ((nameoff + grub_strlen (name) < grub_fdt_get_size_dt_strings (fdt))
 249             && !grub_strcmp (name, (char *) fdt +
 250                              grub_fdt_get_off_dt_strings (fdt) + nameoff))
 251         {
 252           if (prop + prop_entry_size(grub_be_to_cpu32(*(prop + 1)))
 253               / sizeof (*prop) >= end)
 254             return NULL;
 255           return prop;
 256         }
 257         prop += prop_entry_size(grub_be_to_cpu32(*(prop + 1))) / sizeof (*prop);
 258       }
 259     else if (grub_be_to_cpu32(*prop) == FDT_NOP)
 260       prop++;
 261     else
 262       return NULL;
 263   }
 264   return NULL;
 265 }
 266
 267 /* Check the FDT header for consistency and adjust the totalsize field to match
 268    the size allocated for the FDT; if this function is called before the other
 269    functions in this file and returns success, the other functions are
 270    guaranteed not to access memory locations outside the allocated memory. */
 271 int grub_fdt_check_header_nosize (const void *fdt)
 272 {
 273   if (((grub_addr_t) fdt & 0x3) || (grub_fdt_get_magic (fdt) != FDT_MAGIC)
 274       || (grub_fdt_get_version (fdt) < FDT_SUPPORTED_VERSION)
 275       || (grub_fdt_get_last_comp_version (fdt) > FDT_SUPPORTED_VERSION)
 276       || (grub_fdt_get_off_dt_struct (fdt) & 0x00000003)
 277       || (grub_fdt_get_size_dt_struct (fdt) & 0x00000003)
 278       || (grub_fdt_get_off_dt_struct (fdt) + grub_fdt_get_size_dt_struct (fdt)
 279           > grub_fdt_get_totalsize (fdt))
 280       || (grub_fdt_get_off_dt_strings (fdt) + grub_fdt_get_size_dt_strings (fdt)
 281           > grub_fdt_get_totalsize (fdt))
 282       || (grub_fdt_get_off_mem_rsvmap (fdt) & 0x00000007)
 283       || (grub_fdt_get_off_mem_rsvmap (fdt)
 284           > grub_fdt_get_totalsize (fdt) - 2 * sizeof(grub_uint64_t)))
 285     return -1;
 286   return 0;
 287 }
 288
 289 int grub_fdt_check_header (const void *fdt, unsigned int size)
 290 {
 291   if (size < sizeof (grub_fdt_header_t)
 292       || (grub_fdt_get_totalsize (fdt) > size)
 293       || grub_fdt_check_header_nosize (fdt) == -1)
 294     return -1;
 295   return 0;
 296 }
 297
 298 static const grub_uint32_t *
 299 advance_token (const void *fdt, const grub_uint32_t *token, const grub_uint32_t *end, int skip_current)
 300 {
 301   for (; token < end; skip_current = 0)
 302   {
 303     switch (grub_be_to_cpu32 (*token))
 304     {
 305       case FDT_BEGIN_NODE:
 306         if (skip_current)
 307           {
 308             token = get_next_node (fdt, (char *) (token + 1));
 309             continue;
 310           }
 311         char *ptr;
 312         for (ptr = (char *) (token + 1); *ptr && ptr < (char *) end; ptr++);
 313         if (ptr >= (char *) end)
 314           return 0;
 315         return token;
 316       case FDT_PROP:
 317         /* Skip property token and following data (len, nameoff and property
 318            value). */
 319         if (token >= end - 1)
 320           return 0;
 321         token += prop_entry_size(grub_be_to_cpu32(*(token + 1)))
 322                  / sizeof(*token);
 323         break;
 324       case FDT_NOP:
 325         token++;
 326         break;
 327       default:
 328         return 0;
 329     }
 330   }
 331   return 0;
 332 }
 333
 334 int grub_fdt_next_node (const void *fdt, unsigned int currentoffset)
 335 {
 336   const grub_uint32_t *token = (const grub_uint32_t *) fdt + (currentoffset + grub_fdt_get_off_dt_struct (fdt)) / 4;
 337   token = advance_token (fdt, token, (const void *) struct_end (fdt), 1);
 338   if (!token)
 339     return -1;
 340   return (int) ((grub_addr_t) token - (grub_addr_t) fdt
 341                 - grub_fdt_get_off_dt_struct (fdt));
 342 }
 343
 344 int grub_fdt_first_node (const void *fdt, unsigned int parentoffset)
 345 {
 346   const grub_uint32_t *token, *end;
 347   char *node_name;
 348
 349   if (parentoffset & 0x3)
 350     return -1;
 351   token = (const void *) ((grub_addr_t) fdt + grub_fdt_get_off_dt_struct(fdt)
 352                     + parentoffset);
 353   end = (const void *) struct_end (fdt);
 354   if ((token >= end) || (grub_be_to_cpu32(*token) != FDT_BEGIN_NODE))
 355     return -1;
 356   SKIP_NODE_NAME(node_name, token, end);
 357   token = advance_token (fdt, token, end, 0);
 358   if (!token)
 359     return -1;
 360   return (int) ((grub_addr_t) token - (grub_addr_t) fdt
 361                 - grub_fdt_get_off_dt_struct (fdt));
 362 }
 363
 364 /* Find a direct sub-node of a given parent node. */
 365 int grub_fdt_find_subnode (const void *fdt, unsigned int parentoffset,
 366                            const char *name)
 367 {
 368   const grub_uint32_t *token, *end;
 369   const char *node_name;
 370   int skip_current = 0;
 371
 372   if (parentoffset & 0x3)
 373     return -1;
 374   token = (const void *) ((grub_addr_t) fdt + grub_fdt_get_off_dt_struct(fdt)
 375                     + parentoffset);
 376   end = (const void *) struct_end (fdt);
 377   if ((token >= end) || (grub_be_to_cpu32(*token) != FDT_BEGIN_NODE))
 378     return -1;
 379   SKIP_NODE_NAME(node_name, token, end);
 380   while (1) {
 381     token = advance_token (fdt, token, end, skip_current);
 382     if (!token)
 383       return -1;
 384     skip_current = 1;
 385     node_name = (const char *) token + 4;
 386     if (grub_strcmp (node_name, name) == 0)
 387       return (int) ((grub_addr_t) token - (grub_addr_t) fdt
 388                     - grub_fdt_get_off_dt_struct (fdt));
 389   }
 390 }
 391
 392 const char *
 393 grub_fdt_get_nodename (const void *fdt, unsigned int nodeoffset)
 394 {
 395   return (const char *) fdt + grub_fdt_get_off_dt_struct(fdt) + nodeoffset + 4;
 396 }
 397
 398 int grub_fdt_add_subnode (void *fdt, unsigned int parentoffset,
 399                           const char *name)
 400 {
 401   unsigned int entry_size = node_entry_size(name);
 402
 403   if ((parentoffset & 0x3) || (get_free_space (fdt) < entry_size))
 404     return -1;
 405
 406   /* The new node entry will increase the size of the structure block: rearrange
 407      blocks such that there is sufficient free space between the structure and
 408      the strings block, then add the new node entry. */
 409   if (rearrange_blocks (fdt, entry_size) < 0)
 410     return -1;
 411   return add_subnode (fdt, parentoffset, name);
 412 }
 413
 414 const void *
 415 grub_fdt_get_prop (const void *fdt, unsigned int nodeoffset, const char *name,
 416                    grub_uint32_t *len)
 417 {
 418   grub_uint32_t *prop;
 419   if ((nodeoffset >= grub_fdt_get_size_dt_struct (fdt)) || (nodeoffset & 0x3)
 420       || (grub_be_to_cpu32(*(grub_uint32_t *) ((grub_addr_t) fdt
 421                                                + grub_fdt_get_off_dt_struct (fdt) + nodeoffset))
 422           != FDT_BEGIN_NODE))
 423     return 0;
 424   prop = find_prop (fdt, nodeoffset, name);
 425   if (!prop)
 426     return 0;
 427   if (len)
 428     *len = grub_be_to_cpu32 (*(prop + 1));
 429   return prop + 3;
 430 }
 431
 432 int grub_fdt_set_prop (void *fdt, unsigned int nodeoffset, const char *name,
 433                        const void *val, grub_uint32_t len)
 434 {
 435   grub_uint32_t *prop;
 436   int prop_name_present = 0;
 437   grub_uint32_t nameoff = 0;
 438
 439   if ((nodeoffset >= grub_fdt_get_size_dt_struct (fdt)) || (nodeoffset & 0x3)
 440       || (grub_be_to_cpu32(*(grub_uint32_t *) ((grub_addr_t) fdt
 441                            + grub_fdt_get_off_dt_struct (fdt) + nodeoffset))
 442           != FDT_BEGIN_NODE))
 443     return -1;
 444   prop = find_prop (fdt, nodeoffset, name);
 445   if (prop)
 446     {
 447           grub_uint32_t prop_len = ALIGN_UP(grub_be_to_cpu32 (*(prop + 1)),
 448                                         sizeof(grub_uint32_t));
 449           grub_uint32_t i;
 450
 451       prop_name_present = 1;
 452           for (i = 0; i < prop_len / sizeof(grub_uint32_t); i++)
 453         *(prop + 3 + i) = grub_cpu_to_be32_compile_time (FDT_NOP);
 454       if (len > ALIGN_UP(prop_len, sizeof(grub_uint32_t)))
 455         {
 456           /* Length of new property value is greater than the space allocated
 457              for the current value: a new entry needs to be created, so save the
 458              nameoff field of the current entry and replace the current entry
 459              with NOP tokens. */
 460           nameoff = grub_be_to_cpu32 (*(prop + 2));
 461           *prop = *(prop + 1) = *(prop + 2) = grub_cpu_to_be32_compile_time (FDT_NOP);
 462           prop = NULL;
 463         }
 464     }
 465   if (!prop || !prop_name_present) {
 466     unsigned int needed_space = 0;
 467
 468     if (!prop)
 469       needed_space = prop_entry_size(len);
 470     if (!prop_name_present)
 471       needed_space += grub_strlen (name) + 1;
 472     if (needed_space > get_free_space (fdt))
 473       return -1;
 474     if (rearrange_blocks (fdt, !prop ? prop_entry_size(len) : 0) < 0)
 475       return -1;
 476   }
 477   if (!prop_name_present) {
 478     /* Append the property name at the end of the strings block. */
 479     nameoff = grub_fdt_get_size_dt_strings (fdt);
 480     grub_strcpy ((char *) fdt + grub_fdt_get_off_dt_strings (fdt) + nameoff,
 481                  name);
 482     grub_fdt_set_size_dt_strings (fdt, grub_fdt_get_size_dt_strings (fdt)
 483                                   + grub_strlen (name) + 1);
 484   }
 485   if (!prop) {
 486     char *node_name = (char *) ((grub_addr_t) fdt
 487                                 + grub_fdt_get_off_dt_struct (fdt) + nodeoffset
 488                                 + sizeof(grub_uint32_t));
 489
 490     prop = (void *) (node_name + ALIGN_UP(grub_strlen(node_name) + 1, 4));
 491     grub_memmove (prop + prop_entry_size(len) / sizeof(*prop), prop,
 492                   struct_end(fdt) - (grub_addr_t) prop);
 493     grub_fdt_set_size_dt_struct (fdt, grub_fdt_get_size_dt_struct (fdt)
 494                                  + prop_entry_size(len));
 495     *prop = grub_cpu_to_be32_compile_time (FDT_PROP);
 496     *(prop + 2) = grub_cpu_to_be32 (nameoff);
 497   }
 498   *(prop + 1) = grub_cpu_to_be32 (len);
 499
 500   /* Insert padding bytes at the end of the value; if they are not needed, they
 501      will be overwritten by the following memcpy. */
 502   *(prop + prop_entry_size(len) / sizeof(grub_uint32_t) - 1) = 0;
 503
 504   grub_memcpy (prop + 3, val, len);
 505   return 0;
 506 }
 507
 508 int
 509 grub_fdt_create_empty_tree (void *fdt, unsigned int size)
 510 {
 511   struct grub_fdt_empty_tree *et;
 512
 513   if (size < GRUB_FDT_EMPTY_TREE_SZ)
 514     return -1;
 515
 516   grub_memset (fdt, 0, size);
 517   et = fdt;
 518
 519   et->empty_node.tree_end = grub_cpu_to_be32_compile_time (FDT_END);
 520   et->empty_node.node_end = grub_cpu_to_be32_compile_time (FDT_END_NODE);
 521   et->empty_node.node_start = grub_cpu_to_be32_compile_time (FDT_BEGIN_NODE);
 522   ((struct grub_fdt_empty_tree *) fdt)->header.off_mem_rsvmap =
 523     grub_cpu_to_be32_compile_time (ALIGN_UP (sizeof (grub_fdt_header_t), 8));
 524
 525   grub_fdt_set_off_dt_strings (fdt, sizeof (*et));
 526   grub_fdt_set_off_dt_struct (fdt,
 527                               sizeof (et->header) + sizeof (et->empty_rsvmap));
 528   grub_fdt_set_version (fdt, FDT_SUPPORTED_VERSION);
 529   grub_fdt_set_last_comp_version (fdt, FDT_SUPPORTED_VERSION);
 530   grub_fdt_set_size_dt_struct (fdt, sizeof (et->empty_node));
 531   grub_fdt_set_totalsize (fdt, size);
 532   grub_fdt_set_magic (fdt, FDT_MAGIC);
 533
 534   return 0;
 535 }