sd-boot: Support installing new devicetree

The Bootloader Specification says "devicetree refers to the binary
device tree to use when executing the kernel..", but systemd-boot
didn't actually do anything when encountering this stanza until now.

Add support for loading, applying fixups if relevant, and installing the
new device tree before executing the kernel.

src/boot/efi/boot.c

@@ -5,6 +5,7 @@
 #include <efilib.h>
 
 #include "console.h"
+#include "devicetree.h"
 #include "disk.h"
 #include "efi-loader-features.h"
 #include "graphics.h"
@@ -40,6 +41,7 @@ typedef struct {
         EFI_HANDLE *device;
         enum loader_type type;
         CHAR16 *loader;
+        CHAR16 *devicetree;
         CHAR16 *options;
         CHAR16 key;
         EFI_STATUS (*call)(VOID);
@@ -475,6 +477,8 @@ static VOID print_status(Config *config, CHAR16 *loaded_image_path) {
                 }
                 if (entry->loader)
                         Print(L"loader                  '%s'\n", entry->loader);
+                if (entry->devicetree)
+                        Print(L"devicetree              '%s'\n", entry->devicetree);
                 if (entry->options)
                         Print(L"options                 '%s'\n", entry->options);
                 Print(L"auto-select             %s\n", yes_no(!entry->no_autoselect));
@@ -893,6 +897,7 @@ static VOID config_entry_free(ConfigEntry *entry) {
         FreePool(entry->version);
         FreePool(entry->machine_id);
         FreePool(entry->loader);
+        FreePool(entry->devicetree);
         FreePool(entry->options);
         FreePool(entry->path);
         FreePool(entry->current_name);
@@ -1330,6 +1335,12 @@ static VOID config_entry_add_from_file(
                         continue;
                 }
 
+                if (strcmpa((CHAR8 *)"devicetree", key) == 0) {
+                        FreePool(entry->devicetree);
+                        entry->devicetree = stra_to_path(value);
+                        continue;
+                }
+
                 if (strcmpa((CHAR8 *)"initrd", key) == 0) {
                         _cleanup_freepool_ CHAR16 *new = NULL;
 
@@ -2270,10 +2281,12 @@ static VOID config_load_xbootldr(
 }
 
 static EFI_STATUS image_start(
+                EFI_FILE_HANDLE root_dir,
                 EFI_HANDLE parent_image,
                 const Config *config,
                 const ConfigEntry *entry) {
 
+        _cleanup_(devicetree_cleanup) struct devicetree_state dtstate = {};
         EFI_HANDLE image;
         _cleanup_freepool_ EFI_DEVICE_PATH *path = NULL;
         CHAR16 *options;
@@ -2290,6 +2303,12 @@ static EFI_STATUS image_start(
         if (EFI_ERROR(err))
                 return log_error_status_stall(err, L"Error loading %s: %r", entry->loader, err);
 
+        if (entry->devicetree) {
+                err = devicetree_install(&dtstate, root_dir, entry->devicetree);
+                if (EFI_ERROR(err))
+                        return log_error_status_stall(err, L"Error loading %s: %r", entry->devicetree, err);
+        }
+
         if (config->options_edit)
                 options = config->options_edit;
         else if (entry->options)
@@ -2533,7 +2552,7 @@ EFI_STATUS efi_main(EFI_HANDLE image, EFI_SYSTEM_TABLE *sys_table) {
                 (VOID) process_random_seed(root_dir, config.random_seed_mode);
 
                 uefi_call_wrapper(BS->SetWatchdogTimer, 4, 5 * 60, 0x10000, 0, NULL);
-                err = image_start(image, &config, entry);
+                err = image_start(root_dir, image, &config, entry);
                 if (EFI_ERROR(err)) {
                         graphics_mode(FALSE);
                         log_error_stall(L"Failed to execute %s (%s): %r", entry->title, entry->loader, err);

src/boot/efi/devicetree.c

@@ -0,0 +1,131 @@
+/* SPDX-License-Identifier: LGPL-2.1-or-later */
+
+#include <efi.h>
+
+#include "devicetree.h"
+#include "missing_efi.h"
+#include "util.h"
+
+#define FDT_V1_SIZE (7*4)
+
+static EFI_STATUS devicetree_allocate(struct devicetree_state *state, UINTN size) {
+        UINTN pages = DIV_ROUND_UP(size, EFI_PAGE_SIZE);
+        EFI_STATUS err;
+
+        assert(state);
+
+        err = uefi_call_wrapper(BS->AllocatePages, 4, AllocateAnyPages, EfiACPIReclaimMemory, pages,
+                                &state->addr);
+        if (EFI_ERROR(err))
+                return err;
+
+        state->pages = pages;
+        return err;
+}
+
+static UINTN devicetree_allocated(const struct devicetree_state *state) {
+        assert(state);
+        return state->pages * EFI_PAGE_SIZE;
+}
+
+static VOID *devicetree_ptr(const struct devicetree_state *state) {
+        assert(state);
+        assert(state->addr <= UINTN_MAX);
+        return (VOID *)(UINTN)state->addr;
+}
+
+static EFI_STATUS devicetree_fixup(struct devicetree_state *state, UINTN len) {
+        EFI_DT_FIXUP_PROTOCOL *fixup;
+        UINTN size;
+        EFI_STATUS err;
+
+        assert(state);
+
+        err = LibLocateProtocol(&EfiDtFixupProtocol, (VOID **)&fixup);
+        if (EFI_ERROR(err))
+                return log_error_status_stall(EFI_SUCCESS,
+                                              L"Could not locate device tree fixup protocol, skipping.");
+
+        size = devicetree_allocated(state);
+        err = uefi_call_wrapper(fixup->Fixup, 4, fixup, devicetree_ptr(state), &size,
+                                EFI_DT_APPLY_FIXUPS | EFI_DT_RESERVE_MEMORY);
+        if (err == EFI_BUFFER_TOO_SMALL) {
+                EFI_PHYSICAL_ADDRESS oldaddr = state->addr;
+                UINTN oldpages = state->pages;
+                VOID *oldptr = devicetree_ptr(state);
+
+                err = devicetree_allocate(state, size);
+                if (EFI_ERROR(err))
+                        return err;
+
+                CopyMem(devicetree_ptr(state), oldptr, len);
+                err = uefi_call_wrapper(BS->FreePages, 2, oldaddr, oldpages);
+                if (EFI_ERROR(err))
+                        return err;
+
+                size = devicetree_allocated(state);
+                err = uefi_call_wrapper(fixup->Fixup, 4, fixup, devicetree_ptr(state), &size,
+                                        EFI_DT_APPLY_FIXUPS | EFI_DT_RESERVE_MEMORY);
+        }
+
+        return err;
+}
+
+EFI_STATUS devicetree_install(struct devicetree_state *state,
+                EFI_FILE_HANDLE root_dir, CHAR16 *name) {
+        _cleanup_(FileHandleClosep) EFI_FILE_HANDLE handle = NULL;
+        _cleanup_freepool_ EFI_FILE_INFO *info = NULL;
+        UINTN len;
+        EFI_STATUS err;
+
+        assert(state);
+        assert(root_dir);
+        assert(name);
+
+        err = LibGetSystemConfigurationTable(&EfiDtbTableGuid, &state->orig);
+        if (EFI_ERROR(err))
+                return EFI_UNSUPPORTED;
+
+        err = uefi_call_wrapper(root_dir->Open, 5, root_dir, &handle, name, EFI_FILE_MODE_READ,
+                                EFI_FILE_READ_ONLY);
+        if (EFI_ERROR(err))
+                return err;
+
+        info = LibFileInfo(handle);
+        if (!info)
+                return EFI_OUT_OF_RESOURCES;
+        if (info->FileSize < FDT_V1_SIZE || info->FileSize > 32 * 1024 * 1024)
+                /* 32MB device tree blob doesn't seem right */
+                return EFI_INVALID_PARAMETER;
+
+        len = info->FileSize;
+
+        err = devicetree_allocate(state, len);
+        if (EFI_ERROR(err))
+                return err;
+
+        err = uefi_call_wrapper(handle->Read, 3, handle, &len, devicetree_ptr(state));
+        if (EFI_ERROR(err))
+                return err;
+
+        err = devicetree_fixup(state, len);
+        if (EFI_ERROR(err))
+                return err;
+
+        return uefi_call_wrapper(BS->InstallConfigurationTable, 2, &EfiDtbTableGuid, devicetree_ptr(state));
+}
+
+void devicetree_cleanup(struct devicetree_state *state) {
+        EFI_STATUS err;
+
+        if (!state->pages)
+                return;
+
+        err = uefi_call_wrapper(BS->InstallConfigurationTable, 2, &EfiDtbTableGuid, state->orig);
+        /* don't free the current device tree if we can't reinstate the old one */
+        if (EFI_ERROR(err))
+                return;
+
+        uefi_call_wrapper(BS->FreePages, 2, state->addr, state->pages);
+        state->pages = 0;
+}

src/boot/efi/devicetree.h

@@ -0,0 +1,11 @@
+/* SPDX-License-Identifier: LGPL-2.1-or-later */
+#pragma once
+
+struct devicetree_state {
+        EFI_PHYSICAL_ADDRESS addr;
+        UINTN pages;
+        VOID *orig;
+};
+
+EFI_STATUS devicetree_install(struct devicetree_state *state, EFI_FILE_HANDLE root_dir, CHAR16 *name);
+void devicetree_cleanup(struct devicetree_state *state);

src/boot/efi/meson.build

@@ -2,6 +2,7 @@
 
 efi_headers = files('''
         console.h
+        devicetree.h
         disk.h
         graphics.h
         linux.h
@@ -28,6 +29,7 @@ common_sources = '''
 systemd_boot_sources = '''
         boot.c
         console.c
+        devicetree.c
         random-seed.c
         sha256.c
         shim.c

src/boot/efi/missing_efi.h

@@ -124,3 +124,39 @@ typedef struct _EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL {
 #ifndef EFI_IMAGE_MACHINE_RISCV64
         #define EFI_IMAGE_MACHINE_RISCV64 0x5064
 #endif
+
+#ifndef EFI_DTB_TABLE_GUID
+#define EFI_DTB_TABLE_GUID \
+        { 0xb1b621d5, 0xf19c, 0x41a5, {0x83, 0x0b, 0xd9, 0x15, 0x2c, 0x69, 0xaa, 0xe0} }
+#define EfiDtbTableGuid ((EFI_GUID)EFI_DTB_TABLE_GUID)
+#endif
+
+#ifndef EFI_DT_FIXUP_PROTOCOL_GUID
+#define EFI_DT_FIXUP_PROTOCOL_GUID \
+        { 0xe617d64c, 0xfe08, 0x46da, {0xf4, 0xdc, 0xbb, 0xd5, 0x87, 0x0c, 0x73, 0x00} }
+#define EfiDtFixupProtocol ((EFI_GUID)EFI_DT_FIXUP_PROTOCOL_GUID)
+
+#define EFI_DT_FIXUP_PROTOCOL_REVISION 0x00010000
+
+/* Add nodes and update properties */
+#define EFI_DT_APPLY_FIXUPS    0x00000001
+/*
+ * Reserve memory according to the /reserved-memory node
+ * and the memory reservation block
+ */
+#define EFI_DT_RESERVE_MEMORY  0x00000002
+
+typedef struct _EFI_DT_FIXUP_PROTOCOL EFI_DT_FIXUP_PROTOCOL;
+
+typedef EFI_STATUS (EFIAPI *EFI_DT_FIXUP) (
+        IN EFI_DT_FIXUP_PROTOCOL *This,
+        IN VOID                  *Fdt,
+        IN OUT UINTN             *BufferSize,
+        IN UINT32                Flags);
+
+struct _EFI_DT_FIXUP_PROTOCOL {
+        UINT64         Revision;
+        EFI_DT_FIXUP   Fixup;
+};
+
+#endif