vaseboot/VasEBoot-core/loader/arm64/fdt.c

/*
 *  VasEBoot  --  GRand Unified Bootloader
 *  Copyright (C) 2013-2015  Free Software Foundation, Inc.
 *
 *  VasEBoot 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.
 *
 *  VasEBoot 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 VasEBoot.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <VasEBoot/fdt.h>
#include <VasEBoot/mm.h>
#include <VasEBoot/cpu/fdtload.h>
#include <VasEBoot/err.h>
#include <VasEBoot/dl.h>
#include <VasEBoot/command.h>
#include <VasEBoot/file.h>
#include <VasEBoot/efi/efi.h>

static void *loaded_fdt;
static void *fdt;

void *
VasEBoot_fdt_load (VasEBoot_size_t additional_size)
{
  void *raw_fdt;
  VasEBoot_size_t size;

  if (fdt)
    {
      size = VasEBoot_EFI_BYTES_TO_PAGES (VasEBoot_fdt_get_totalsize (fdt));
      VasEBoot_efi_free_pages ((VasEBoot_efi_physical_address_t) fdt, size);
    }

  if (loaded_fdt)
    raw_fdt = loaded_fdt;
  else
    raw_fdt = VasEBoot_efi_get_firmware_fdt();

  size =
    raw_fdt ? VasEBoot_fdt_get_totalsize (raw_fdt) : VasEBoot_FDT_EMPTY_TREE_SZ;
  size += additional_size;

  VasEBoot_dprintf ("linux", "allocating %ld bytes for fdt\n", size);
  fdt = VasEBoot_efi_allocate_pages (0, VasEBoot_EFI_BYTES_TO_PAGES (size));
  if (!fdt)
    return NULL;

  if (raw_fdt)
    {
      VasEBoot_memmove (fdt, raw_fdt, size);
      VasEBoot_fdt_set_totalsize (fdt, size);
    }
  else
    {
      VasEBoot_fdt_create_empty_tree (fdt, size);
    }
  return fdt;
}

VasEBoot_err_t
VasEBoot_fdt_install (void)
{
  VasEBoot_efi_boot_services_t *b;
  VasEBoot_efi_guid_t fdt_guid = VasEBoot_EFI_DEVICE_TREE_GUID;
  VasEBoot_efi_status_t status;

  b = VasEBoot_efi_system_table->boot_services;
  status = b->install_configuration_table (&fdt_guid, fdt);
  if (status != VasEBoot_EFI_SUCCESS)
    return VasEBoot_error (VasEBoot_ERR_IO, "failed to install FDT");

  VasEBoot_dprintf ("fdt", "Installed/updated FDT configuration table @ %p\n",
		fdt);
  return VasEBoot_ERR_NONE;
}

void
VasEBoot_fdt_unload (void) {
  if (!fdt) {
    return;
  }
  VasEBoot_efi_free_pages ((VasEBoot_efi_physical_address_t) fdt,
		       VasEBoot_EFI_BYTES_TO_PAGES (VasEBoot_fdt_get_totalsize (fdt)));
  fdt = NULL;
}

static VasEBoot_err_t
VasEBoot_cmd_devicetree (VasEBoot_command_t cmd __attribute__ ((unused)),
		     int argc, char *argv[])
{
  VasEBoot_file_t dtb;
  void *blob = NULL;
  int size;

  if (loaded_fdt)
    VasEBoot_free (loaded_fdt);
  loaded_fdt = NULL;

  /* No arguments means "use firmware FDT".  */
  if (argc == 0)
    {
      return VasEBoot_ERR_NONE;
    }

  dtb = VasEBoot_file_open (argv[0]);
  if (!dtb)
    goto out;

  size = VasEBoot_file_size (dtb);
  blob = VasEBoot_malloc (size);
  if (!blob)
    goto out;

  if (VasEBoot_file_read (dtb, blob, size) < size)
    {
      if (!VasEBoot_errno)
	VasEBoot_error (VasEBoot_ERR_BAD_OS, N_("premature end of file %s"), argv[0]);
      goto out;
    }

  if (VasEBoot_fdt_check_header (blob, size) != 0)
    {
      VasEBoot_error (VasEBoot_ERR_BAD_OS, N_("invalid device tree"));
      goto out;
    }

out:
  if (dtb)
    VasEBoot_file_close (dtb);

  if (blob)
    {
      if (VasEBoot_errno == VasEBoot_ERR_NONE)
	loaded_fdt = blob;
      else
	VasEBoot_free (blob);
    }

  return VasEBoot_errno;
}

static VasEBoot_command_t cmd_devicetree;

VasEBoot_MOD_INIT (fdt)
{
  cmd_devicetree =
    VasEBoot_register_command ("devicetree", VasEBoot_cmd_devicetree, 0,
			   N_("Load DTB file."));
}

VasEBoot_MOD_FINI (fdt)
{
  VasEBoot_unregister_command (cmd_devicetree);
}