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           ;
 314         if (ptr >= (char *) end)
 315           return 0;
 316         return token;
 317       case FDT_PROP:
 318         /* Skip property token and following data (len, nameoff and property
 319            value). */
 320         if (token >= end - 1)
 321           return 0;
 322         token += prop_entry_size(grub_be_to_cpu32(*(token + 1)))
 323                  / sizeof(*token);
 324         break;
 325       case FDT_NOP:
 326         token++;
 327         break;
 328       default:
 329         return 0;
 330     }
 331   }
 332   return 0;
 333 }
 334
 335 int grub_fdt_next_node (const void *fdt, unsigned int currentoffset)
 336 {
 337   const grub_uint32_t *token = (const grub_uint32_t *) fdt + (currentoffset + grub_fdt_get_off_dt_struct (fdt)) / 4;
 338   token = advance_token (fdt, token, (const void *) struct_end (fdt), 1);
 339   if (!token)
 340     return -1;
 341   return (int) ((grub_addr_t) token - (grub_addr_t) fdt
 342                 - grub_fdt_get_off_dt_struct (fdt));
 343 }
 344
 345 int grub_fdt_first_node (const void *fdt, unsigned int parentoffset)
 346 {
 347   const grub_uint32_t *token, *end;
 348   char *node_name;
 349
 350   if (parentoffset & 0x3)
 351     return -1;
 352   token = (const void *) ((grub_addr_t) fdt + grub_fdt_get_off_dt_struct(fdt)
 353                     + parentoffset);
 354   end = (const void *) struct_end (fdt);
 355   if ((token >= end) || (grub_be_to_cpu32(*token) != FDT_BEGIN_NODE))
 356     return -1;
 357   SKIP_NODE_NAME(node_name, token, end);
 358   token = advance_token (fdt, token, end, 0);
 359   if (!token)
 360     return -1;
 361   return (int) ((grub_addr_t) token - (grub_addr_t) fdt
 362                 - grub_fdt_get_off_dt_struct (fdt));
 363 }
 364
 365 /* Find a direct sub-node of a given parent node. */
 366 int grub_fdt_find_subnode (const void *fdt, unsigned int parentoffset,
 367                            const char *name)
 368 {
 369   const grub_uint32_t *token, *end;
 370   const char *node_name;
 371   int skip_current = 0;
 372
 373   if (parentoffset & 0x3)
 374     return -1;
 375   token = (const void *) ((grub_addr_t) fdt + grub_fdt_get_off_dt_struct(fdt)
 376                     + parentoffset);
 377   end = (const void *) struct_end (fdt);
 378   if ((token >= end) || (grub_be_to_cpu32(*token) != FDT_BEGIN_NODE))
 379     return -1;
 380   SKIP_NODE_NAME(node_name, token, end);
 381   while (1) {
 382     token = advance_token (fdt, token, end, skip_current);
 383     if (!token)
 384       return -1;
 385     skip_current = 1;
 386     node_name = (const char *) token + 4;
 387     if (grub_strcmp (node_name, name) == 0)
 388       return (int) ((grub_addr_t) token - (grub_addr_t) fdt
 389                     - grub_fdt_get_off_dt_struct (fdt));
 390   }
 391 }
 392
 393 const char *
 394 grub_fdt_get_nodename (const void *fdt, unsigned int nodeoffset)
 395 {
 396   return (const char *) fdt + grub_fdt_get_off_dt_struct(fdt) + nodeoffset + 4;
 397 }
 398
 399 int grub_fdt_add_subnode (void *fdt, unsigned int parentoffset,
 400                           const char *name)
 401 {
 402   unsigned int entry_size = node_entry_size(name);
 403
 404   if ((parentoffset & 0x3) || (get_free_space (fdt) < entry_size))
 405     return -1;
 406
 407   /* The new node entry will increase the size of the structure block: rearrange
 408      blocks such that there is sufficient free space between the structure and
 409      the strings block, then add the new node entry. */
 410   if (rearrange_blocks (fdt, entry_size) < 0)
 411     return -1;
 412   return add_subnode (fdt, parentoffset, name);
 413 }
 414
 415 const void *
 416 grub_fdt_get_prop (const void *fdt, unsigned int nodeoffset, const char *name,
 417                    grub_uint32_t *len)
 418 {
 419   grub_uint32_t *prop;
 420   if ((nodeoffset >= grub_fdt_get_size_dt_struct (fdt)) || (nodeoffset & 0x3)
 421       || (grub_be_to_cpu32(*(grub_uint32_t *) ((grub_addr_t) fdt
 422                                                + grub_fdt_get_off_dt_struct (fdt) + nodeoffset))
 423           != FDT_BEGIN_NODE))
 424     return 0;
 425   prop = find_prop (fdt, nodeoffset, name);
 426   if (!prop)
 427     return 0;
 428   if (len)
 429     *len = grub_be_to_cpu32 (*(prop + 1));
 430   return prop + 3;
 431 }
 432
 433 int grub_fdt_set_prop (void *fdt, unsigned int nodeoffset, const char *name,
 434                        const void *val, grub_uint32_t len)
 435 {
 436   grub_uint32_t *prop;
 437   int prop_name_present = 0;
 438   grub_uint32_t nameoff = 0;
 439
 440   if ((nodeoffset >= grub_fdt_get_size_dt_struct (fdt)) || (nodeoffset & 0x3)
 441       || (grub_be_to_cpu32(*(grub_uint32_t *) ((grub_addr_t) fdt
 442                            + grub_fdt_get_off_dt_struct (fdt) + nodeoffset))
 443           != FDT_BEGIN_NODE))
 444     return -1;
 445   prop = find_prop (fdt, nodeoffset, name);
 446   if (prop)
 447     {
 448           grub_uint32_t prop_len = ALIGN_UP(grub_be_to_cpu32 (*(prop + 1)),
 449                                         sizeof(grub_uint32_t));
 450           grub_uint32_t i;
 451
 452       prop_name_present = 1;
 453           for (i = 0; i < prop_len / sizeof(grub_uint32_t); i++)
 454         *(prop + 3 + i) = grub_cpu_to_be32_compile_time (FDT_NOP);
 455       if (len > ALIGN_UP(prop_len, sizeof(grub_uint32_t)))
 456         {
 457           /* Length of new property value is greater than the space allocated
 458              for the current value: a new entry needs to be created, so save the
 459              nameoff field of the current entry and replace the current entry
 460              with NOP tokens. */
 461           nameoff = grub_be_to_cpu32 (*(prop + 2));
 462           *prop = *(prop + 1) = *(prop + 2) = grub_cpu_to_be32_compile_time (FDT_NOP);
 463           prop = NULL;
 464         }
 465     }
 466   if (!prop || !prop_name_present) {
 467     unsigned int needed_space = 0;
 468
 469     if (!prop)
 470       needed_space = prop_entry_size(len);
 471     if (!prop_name_present)
 472       needed_space += grub_strlen (name) + 1;
 473     if (needed_space > get_free_space (fdt))
 474       return -1;
 475     if (rearrange_blocks (fdt, !prop ? prop_entry_size(len) : 0) < 0)
 476       return -1;
 477   }
 478   if (!prop_name_present) {
 479     /* Append the property name at the end of the strings block. */
 480     nameoff = grub_fdt_get_size_dt_strings (fdt);
 481     grub_strcpy ((char *) fdt + grub_fdt_get_off_dt_strings (fdt) + nameoff,
 482                  name);
 483     grub_fdt_set_size_dt_strings (fdt, grub_fdt_get_size_dt_strings (fdt)
 484                                   + grub_strlen (name) + 1);
 485   }
 486   if (!prop) {
 487     char *node_name = (char *) ((grub_addr_t) fdt
 488                                 + grub_fdt_get_off_dt_struct (fdt) + nodeoffset
 489                                 + sizeof(grub_uint32_t));
 490
 491     prop = (void *) (node_name + ALIGN_UP(grub_strlen(node_name) + 1, 4));
 492     grub_memmove (prop + prop_entry_size(len) / sizeof(*prop), prop,
 493                   struct_end(fdt) - (grub_addr_t) prop);
 494     grub_fdt_set_size_dt_struct (fdt, grub_fdt_get_size_dt_struct (fdt)
 495                                  + prop_entry_size(len));
 496     *prop = grub_cpu_to_be32_compile_time (FDT_PROP);
 497     *(prop + 2) = grub_cpu_to_be32 (nameoff);
 498   }
 499   *(prop + 1) = grub_cpu_to_be32 (len);
 500
 501   /* Insert padding bytes at the end of the value; if they are not needed, they
 502      will be overwritten by the following memcpy. */
 503   *(prop + prop_entry_size(len) / sizeof(grub_uint32_t) - 1) = 0;
 504
 505   grub_memcpy (prop + 3, val, len);
 506   return 0;
 507 }
 508
 509 int
 510 grub_fdt_create_empty_tree (void *fdt, unsigned int size)
 511 {
 512   struct grub_fdt_empty_tree *et;
 513
 514   if (size < GRUB_FDT_EMPTY_TREE_SZ)
 515     return -1;
 516
 517   grub_memset (fdt, 0, size);
 518   et = fdt;
 519
 520   et->empty_node.tree_end = grub_cpu_to_be32_compile_time (FDT_END);
 521   et->empty_node.node_end = grub_cpu_to_be32_compile_time (FDT_END_NODE);
 522   et->empty_node.node_start = grub_cpu_to_be32_compile_time (FDT_BEGIN_NODE);
 523   ((struct grub_fdt_empty_tree *) fdt)->header.off_mem_rsvmap =
 524     grub_cpu_to_be32_compile_time (ALIGN_UP (sizeof (grub_fdt_header_t), 8));
 525
 526   grub_fdt_set_off_dt_strings (fdt, sizeof (*et));
 527   grub_fdt_set_off_dt_struct (fdt,
 528                               sizeof (et->header) + sizeof (et->empty_rsvmap));
 529   grub_fdt_set_version (fdt, FDT_SUPPORTED_VERSION);
 530   grub_fdt_set_last_comp_version (fdt, FDT_SUPPORTED_VERSION);
 531   grub_fdt_set_size_dt_struct (fdt, sizeof (et->empty_node));
 532   grub_fdt_set_totalsize (fdt, size);
 533   grub_fdt_set_magic (fdt, FDT_MAGIC);
 534
 535   return 0;
 536 }