Merge tag 'efi-next' of https://gitlab.denx.de/u-boot/custodians/u-boot-efi into nextWIP/01Jan2021-next

Pull request for UEFI sub-system for next

* UEFI capsule authentication
* UEFI capsule update on QEMU ARM
* fsuuid command for FAT file system
* bug fixes

36 files changed, 1240 insertions, 205 deletions

diff --git a/arch/arm/mach-qemu/Kconfig b/arch/arm/mach-qemu/Kconfig
index 588d2d3..186c358 100644
--- a/arch/arm/mach-qemu/Kconfig
+++ b/arch/arm/mach-qemu/Kconfig
@@ -16,12 +16,14 @@ choice
 config TARGET_QEMU_ARM_32BIT
 	bool "ARMv7-A, 32bit"
 	select ARCH_SUPPORT_PSCI
+	select BOARD_LATE_INIT
 	select CPU_V7A
 	select SYS_ARCH_TIMER
 
 config TARGET_QEMU_ARM_64BIT
 	bool "ARMv8, 64bit"
 	select ARM64
+	select BOARD_LATE_INIT
 
 endchoice
 
diff --git a/arch/x86/include/asm/fsp/fsp_support.h b/arch/x86/include/asm/fsp/fsp_support.h
index 29e5114..3cd3e4f 100644
--- a/arch/x86/include/asm/fsp/fsp_support.h
+++ b/arch/x86/include/asm/fsp/fsp_support.h
@@ -7,11 +7,12 @@
 #ifndef __FSP_SUPPORT_H__
 #define __FSP_SUPPORT_H__
 
+#include <signatures.h>
+
 #include <asm/fsp/fsp_bootmode.h>
 #include <asm/fsp/fsp_fv.h>
 #include <asm/fsp/fsp_hob.h>
 #include <asm/fsp/fsp_infoheader.h>
-#include <asm/fsp/fsp_types.h>
 #include <asm/fsp_arch.h>
 #include <asm/fsp/fsp_azalia.h>
 
diff --git a/board/emulation/common/Kconfig b/board/emulation/common/Kconfig
new file mode 100644
index 0000000..4c15c8b
--- /dev/null
+++ b/board/emulation/common/Kconfig
@@ -0,0 +1,15 @@
+config MTDPARTS_NOR0
+	string "mtd boot partition for nor0"
+	default "64m(u-boot)" if TARGET_QEMU_ARM_64BIT && !TFABOOT
+	depends on SYS_MTDPARTS_RUNTIME
+	help
+	  This define the partition of nor0 used to build mtparts dynamically
+	  for boot from nor0.
+
+config MTDPARTS_NOR1
+	string "mtd u-boot env partition for nor1"
+	default "64m(u-boot-env)" if TARGET_QEMU_ARM_64BIT && !TFABOOT
+	depends on SYS_MTDPARTS_RUNTIME
+	help
+	  This define the partition of nor1 used to build mtparts dynamically
+	  for the u-boot env stored on nor1.
diff --git a/board/emulation/common/Makefile b/board/emulation/common/Makefile
new file mode 100644
index 0000000..7ed447a
--- /dev/null
+++ b/board/emulation/common/Makefile
@@ -0,0 +1,5 @@
+# SPDX-License-Identifier: GPL-2.0+
+
+obj-$(CONFIG_SYS_MTDPARTS_RUNTIME) += qemu_mtdparts.o
+obj-$(CONFIG_SET_DFU_ALT_INFO) += qemu_dfu.o
+obj-$(CONFIG_EFI_CAPSULE_FIRMWARE_MANAGEMENT) += qemu_capsule.o
diff --git a/board/emulation/common/qemu_capsule.c b/board/emulation/common/qemu_capsule.c
new file mode 100644
index 0000000..f1d4035
--- /dev/null
+++ b/board/emulation/common/qemu_capsule.c
@@ -0,0 +1,48 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 2020 Linaro Limited
+ */
+
+#include <common.h>
+#include <efi_api.h>
+#include <efi_loader.h>
+#include <env.h>
+#include <fdtdec.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+int efi_get_public_key_data(void **pkey, efi_uintn_t *pkey_len)
+{
+	const void *fdt_blob = gd->fdt_blob;
+	const void *blob;
+	const char *cnode_name = "capsule-key";
+	const char *snode_name = "signature";
+	int sig_node;
+	int len;
+
+	sig_node = fdt_subnode_offset(fdt_blob, 0, snode_name);
+	if (sig_node < 0) {
+		EFI_PRINT("Unable to get signature node offset\n");
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	blob = fdt_getprop(fdt_blob, sig_node, cnode_name, &len);
+
+	if (!blob || len < 0) {
+		EFI_PRINT("Unable to get capsule-key value\n");
+		*pkey = NULL;
+		*pkey_len = 0;
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	*pkey = (void *)blob;
+	*pkey_len = len;
+
+	return 0;
+}
+
+bool efi_capsule_auth_enabled(void)
+{
+	return env_get("capsule_authentication_enabled") != NULL ?
+		true : false;
+}
diff --git a/board/emulation/common/qemu_dfu.c b/board/emulation/common/qemu_dfu.c
new file mode 100644
index 0000000..62234a7
--- /dev/null
+++ b/board/emulation/common/qemu_dfu.c
@@ -0,0 +1,68 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 2020 Linaro Limited
+ */
+
+#include <common.h>
+#include <dfu.h>
+#include <env.h>
+#include <memalign.h>
+#include <mtd.h>
+
+#define DFU_ALT_BUF_LEN		SZ_1K
+
+static void board_get_alt_info(struct mtd_info *mtd, char *buf)
+{
+	struct mtd_info *part;
+	bool first = true;
+	const char *name;
+	int len, partnum = 0;
+
+	name = mtd->name;
+	len = strlen(buf);
+
+	if (buf[0] != '\0')
+		len += snprintf(buf + len, DFU_ALT_BUF_LEN - len, "&");
+	len += snprintf(buf + len, DFU_ALT_BUF_LEN - len,
+			"mtd %s=", name);
+
+	list_for_each_entry(part, &mtd->partitions, node) {
+		partnum++;
+		if (!first)
+			len += snprintf(buf + len, DFU_ALT_BUF_LEN - len, ";");
+		first = false;
+
+		len += snprintf(buf + len, DFU_ALT_BUF_LEN - len,
+				"%s part %d",
+				part->name, partnum);
+	}
+}
+
+void set_dfu_alt_info(char *interface, char *devstr)
+{
+	struct mtd_info *mtd;
+
+	ALLOC_CACHE_ALIGN_BUFFER(char, buf, DFU_ALT_BUF_LEN);
+
+	if (env_get("dfu_alt_info"))
+		return;
+
+	memset(buf, 0, sizeof(buf));
+
+	/*
+	 * Currently dfu_alt_info is needed on Qemu ARM64 for
+	 * capsule updates
+	*/
+	if (IS_ENABLED(CONFIG_EFI_CAPSULE_FIRMWARE_MANAGEMENT) &&
+	    IS_ENABLED(CONFIG_TARGET_QEMU_ARM_64BIT)) {
+		/* probe all MTD devices */
+		mtd_probe_devices();
+
+		mtd = get_mtd_device_nm("nor0");
+		if (!IS_ERR_OR_NULL(mtd))
+			board_get_alt_info(mtd, buf);
+	}
+
+	env_set("dfu_alt_info", buf);
+	printf("dfu_alt_info set\n");
+}
diff --git a/board/emulation/common/qemu_mtdparts.c b/board/emulation/common/qemu_mtdparts.c
new file mode 100644
index 0000000..60212e9
--- /dev/null
+++ b/board/emulation/common/qemu_mtdparts.c
@@ -0,0 +1,82 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 2020 Linaro Limited
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <mtd.h>
+
+#include <linux/string.h>
+
+#define MTDPARTS_LEN		256
+#define MTDIDS_LEN		128
+
+static void board_get_mtdparts(const char *dev, const char *partition,
+			       char *mtdids, char *mtdparts)
+{
+	/* mtdids: "<dev>=<dev>, ...." */
+	if (mtdids[0] != '\0')
+		strcat(mtdids, ",");
+	strcat(mtdids, dev);
+	strcat(mtdids, "=");
+	strcat(mtdids, dev);
+
+	/* mtdparts: "mtdparts=<dev>:<mtdparts_<dev>>;..." */
+	if (mtdparts[0] != '\0')
+		strncat(mtdparts, ";", MTDPARTS_LEN);
+	else
+		strcat(mtdparts, "mtdparts=");
+
+	strncat(mtdparts, dev, MTDPARTS_LEN);
+	strncat(mtdparts, ":", MTDPARTS_LEN);
+	strncat(mtdparts, partition, MTDPARTS_LEN);
+}
+
+void board_mtdparts_default(const char **mtdids, const char **mtdparts)
+{
+	struct mtd_info *mtd;
+	struct udevice *dev;
+	const char *mtd_partition;
+	static char parts[3 * MTDPARTS_LEN + 1];
+	static char ids[MTDIDS_LEN + 1];
+	static bool mtd_initialized;
+
+	if (mtd_initialized) {
+		*mtdids = ids;
+		*mtdparts = parts;
+		return;
+	}
+
+	memset(parts, 0, sizeof(parts));
+	memset(ids, 0, sizeof(ids));
+
+	/* Currently mtdparts is needed on Qemu ARM64 for capsule updates */
+	if (IS_ENABLED(CONFIG_EFI_CAPSULE_FIRMWARE_MANAGEMENT) &&
+	    IS_ENABLED(CONFIG_TARGET_QEMU_ARM_64BIT)) {
+		/* probe all MTD devices */
+		for (uclass_first_device(UCLASS_MTD, &dev); dev;
+		     uclass_next_device(&dev)) {
+			debug("mtd device = %s\n", dev->name);
+		}
+
+		mtd = get_mtd_device_nm("nor0");
+		if (!IS_ERR_OR_NULL(mtd)) {
+			mtd_partition = CONFIG_MTDPARTS_NOR0;
+			board_get_mtdparts("nor0", mtd_partition, ids, parts);
+			put_mtd_device(mtd);
+		}
+
+		mtd = get_mtd_device_nm("nor1");
+		if (!IS_ERR_OR_NULL(mtd)) {
+			mtd_partition = CONFIG_MTDPARTS_NOR1;
+			board_get_mtdparts("nor1", mtd_partition, ids, parts);
+			put_mtd_device(mtd);
+		}
+	}
+
+	mtd_initialized = true;
+	*mtdids = ids;
+	*mtdparts = parts;
+	debug("%s:mtdids=%s & mtdparts=%s\n", __func__, ids, parts);
+}
diff --git a/board/emulation/qemu-arm/Kconfig b/board/emulation/qemu-arm/Kconfig
index 02ae4d9..fb8d38f 100644
--- a/board/emulation/qemu-arm/Kconfig
+++ b/board/emulation/qemu-arm/Kconfig
@@ -11,3 +11,11 @@ config BOARD_SPECIFIC_OPTIONS # dummy
 	imply VIRTIO_BLK
 
 endif
+
+if TARGET_QEMU_ARM_64BIT && !TFABOOT
+config BOARD_SPECIFIC_OPTIONS
+	imply SYS_MTDPARTS_RUNTIME
+	imply SET_DFU_ALT_INFO
+
+source "board/emulation/common/Kconfig"
+endif
diff --git a/board/emulation/qemu-arm/qemu-arm.c b/board/emulation/qemu-arm/qemu-arm.c
index f18f2ed..aa68bef 100644
--- a/board/emulation/qemu-arm/qemu-arm.c
+++ b/board/emulation/qemu-arm/qemu-arm.c
@@ -65,6 +65,11 @@ struct mm_region *mem_map = qemu_arm64_mem_map;
 
 int board_init(void)
 {
+	return 0;
+}
+
+int board_late_init(void)
+{
 	/*
 	 * Make sure virtio bus is enumerated so that peripherals
 	 * on the virtio bus can be discovered by their drivers
diff --git a/cmd/efidebug.c b/cmd/efidebug.c
index fa9d7fe..5fb7b1e 100644
--- a/cmd/efidebug.c
+++ b/cmd/efidebug.c
@@ -79,6 +79,16 @@ static int do_efi_capsule_update(struct cmd_tbl *cmdtp, int flag,
 	return CMD_RET_SUCCESS;
 }
 
+static int do_efi_capsule_on_disk_update(struct cmd_tbl *cmdtp, int flag,
+					 int argc, char * const argv[])
+{
+	efi_status_t ret;
+
+	ret = efi_launch_capsules();
+
+	return ret == EFI_SUCCESS ? CMD_RET_SUCCESS : CMD_RET_FAILURE;
+}
+
 /**
  * do_efi_capsule_show() - show capsule information
  *
@@ -207,6 +217,8 @@ static struct cmd_tbl cmd_efidebug_capsule_sub[] = {
 			 "", ""),
 	U_BOOT_CMD_MKENT(show, CONFIG_SYS_MAXARGS, 1, do_efi_capsule_show,
 			 "", ""),
+	U_BOOT_CMD_MKENT(disk-update, 0, 0, do_efi_capsule_on_disk_update,
+			 "", ""),
 	U_BOOT_CMD_MKENT(result, CONFIG_SYS_MAXARGS, 1, do_efi_capsule_res,
 			 "", ""),
 };
@@ -1544,6 +1556,8 @@ static char efidebug_help_text[] =
 #ifdef CONFIG_EFI_HAVE_CAPSULE_SUPPORT
 	"efidebug capsule update [-v] <capsule address>\n"
 	"  - process a capsule\n"
+	"efidebug capsule disk-update\n"
+	"  - update a capsule from disk\n"
 	"efidebug capsule show <capsule address>\n"
 	"  - show capsule information\n"
 	"efidebug capsule result [<capsule result var>]\n"
diff --git a/doc/board/emulation/index.rst b/doc/board/emulation/index.rst
index 1adefee..a09ead1 100644
--- a/doc/board/emulation/index.rst
+++ b/doc/board/emulation/index.rst
@@ -10,3 +10,4 @@ Emulation
    qemu-mips
    qemu-riscv
    qemu-x86
+   qemu_capsule_update
diff --git a/doc/board/emulation/qemu_capsule_update.rst b/doc/board/emulation/qemu_capsule_update.rst
new file mode 100644
index 0000000..9fec75f
--- /dev/null
+++ b/doc/board/emulation/qemu_capsule_update.rst
@@ -0,0 +1,210 @@
+.. SPDX-License-Identifier: GPL-2.0+
+.. Copyright (C) 2020, Linaro Limited
+
+Enabling UEFI Capsule Update feature
+------------------------------------
+
+Support has been added for the UEFI capsule update feature which
+enables updating the U-Boot image using the UEFI firmware management
+protocol (fmp). The capsules are not passed to the firmware through
+the UpdateCapsule runtime service. Instead, capsule-on-disk
+functionality is used for fetching the capsule from the EFI System
+Partition (ESP) by placing the capsule file under the
+\EFI\UpdateCapsule directory.
+
+Currently, support has been added on the QEMU ARM64 virt platform for
+updating the U-Boot binary as a raw image when the platform is booted
+in non-secure mode, i.e. with CONFIG_TFABOOT disabled. For this
+configuration, the QEMU platform needs to be booted with
+'secure=off'. The U-Boot binary placed on the first bank of the NOR
+flash at offset 0x0. The U-Boot environment is placed on the second
+NOR flash bank at offset 0x4000000.
+
+The capsule update feature is enabled with the following configuration
+settings::
+
+    CONFIG_MTD=y
+    CONFIG_FLASH_CFI_MTD=y
+    CONFIG_CMD_MTDPARTS=y
+    CONFIG_CMD_DFU=y
+    CONFIG_DFU_MTD=y
+    CONFIG_PCI_INIT_R=y
+    CONFIG_EFI_CAPSULE_ON_DISK=y
+    CONFIG_EFI_CAPSULE_FIRMWARE_MANAGEMENT=y
+    CONFIG_EFI_CAPSULE_FIRMWARE=y
+    CONFIG_EFI_CAPSULE_FIRMWARE_RAW=y
+    CONFIG_EFI_CAPSULE_FMP_HEADER=y
+
+In addition, the following config needs to be disabled(QEMU ARM specific)::
+
+    CONFIG_TFABOOT
+
+The capsule file can be generated by using the GenerateCapsule.py
+script in EDKII::
+
+    $ ./BaseTools/BinWrappers/PosixLike/GenerateCapsule -e -o \
+    <capsule_file_name> --fw-version <val> --lsv <val> --guid \
+    e2bb9c06-70e9-4b14-97a3-5a7913176e3f --verbose --update-image-index \
+    <val> --verbose <u-boot.bin>
+
+The above is a wrapper script(GenerateCapsule) which eventually calls
+the actual GenerateCapsule.py script.
+
+As per the UEFI specification, the capsule file needs to be placed on
+the EFI System Partition, under the \EFI\UpdateCapsule directory. The
+EFI System Partition can be a virtio-blk-device.
+
+Before initiating the firmware update, the efi variables BootNext,
+BootXXXX and OsIndications need to be set. The BootXXXX variable needs
+to be pointing to the EFI System Partition which contains the capsule
+file. The BootNext, BootXXXX and OsIndications variables can be set
+using the following commands::
+
+    => efidebug boot add 0 Boot0000 virtio 0:1 <capsule_file_name>
+    => efidebug boot next 0
+    => setenv -e -nv -bs -rt -v OsIndications =0x04
+    => saveenv
+
+Finally, the capsule update can be initiated with the following
+command::
+
+    => efidebug capsule disk-update
+
+The updated U-Boot image will be booted on subsequent boot.
+
+Enabling Capsule Authentication
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+The UEFI specification defines a way of authenticating the capsule to
+be updated by verifying the capsule signature. The capsule signature
+is computed and prepended to the capsule payload at the time of
+capsule generation. This signature is then verified by using the
+public key stored as part of the X509 certificate. This certificate is
+in the form of an efi signature list (esl) file, which is embedded as
+part of the platform's device tree blob using the mkeficapsule
+utility.
+
+On the QEMU virt platforms, the device-tree is generated on the fly
+based on the devices configured. This device tree is then passed on to
+the various software components booting on the platform, including
+U-Boot. Therefore, on the QEMU virt platform, the signatute is
+embedded on an overlay. This overlay is then applied at runtime to the
+base platform device-tree. Steps needed for embedding the esl file in
+the overlay are highlighted below.
+
+The capsule authentication feature can be enabled through the
+following config, in addition to the configs listed above for capsule
+update::
+
+    CONFIG_EFI_CAPSULE_AUTHENTICATE=y
+
+The public and private keys used for the signing process are generated
+and used by the steps highlighted below::
+
+    1. Install utility commands on your host
+       * OPENSSL
+       * efitools
+
+    2. Create signing keys and certificate files on your host
+
+        $ openssl req -x509 -sha256 -newkey rsa:2048 -subj /CN=CRT/ \
+            -keyout CRT.key -out CRT.crt -nodes -days 365
+        $ cert-to-efi-sig-list CRT.crt CRT.esl
+
+        $ openssl x509 -in CRT.crt -out CRT.cer -outform DER
+        $ openssl x509 -inform DER -in CRT.cer -outform PEM -out CRT.pub.pem
+
+        $ openssl pkcs12 -export -out CRT.pfx -inkey CRT.key -in CRT.crt
+        $ openssl pkcs12 -in CRT.pfx -nodes -out CRT.pem
+
+The capsule file can be generated by using the GenerateCapsule.py
+script in EDKII::
+
+    $ ./BaseTools/BinWrappers/PosixLike/GenerateCapsule -e -o \
+      <capsule_file_name> --monotonic-count <val> --fw-version \
+      <val> --lsv <val> --guid \
+      e2bb9c06-70e9-4b14-97a3-5a7913176e3f --verbose \
+      --update-image-index <val> --signer-private-cert \
+      /path/to/CRT.pem --trusted-public-cert \
+      /path/to/CRT.pub.pem --other-public-cert /path/to/CRT.pub.pem \
+      <u-boot.bin>
+
+Place the capsule generated in the above step on the EFI System
+Partition under the EFI/UpdateCapsule directory
+
+For embedding the public key certificate, the following steps need to
+be followed::
+
+    1. Generate a skeleton overlay dts file, with a single fragment
+       node and an empty __overlay__ node
+
+       A typical skeleton overlay file will look like this
+
+       /dts-v1/;
+       /plugin/;
+
+       / {
+               fragment@0 {
+                       target-path = "/";
+                       __overlay__ {
+                       };
+               };
+       };
+
+
+    2. Convert the dts to a corresponding dtb with the following
+       command
+        ./scripts/dtc/dtc -@ -I dts -O dtb -o <ov_dtb_file_name> \
+        <dts_file>
+
+    3. Run the dtb file generated above through the mkeficapsule tool
+       in U-Boot
+        ./tools/mkeficapsule -O <pub_key.esl> -D <ov_dtb>
+
+Running the above command results in the creation of a 'signature'
+node in the dtb, under which the public key is stored as a
+'capsule-key' property. The '-O' option is to be used since the
+public key certificate(esl) file is being embedded in an overlay.
+
+The dtb file embedded with the certificate is now to be placed on an
+EFI System Partition. This would then be loaded and "merged" with the
+base platform flattened device-tree(dtb) at runtime.
+
+Build U-Boot with the following steps(QEMU ARM64)::
+
+    $ make qemu_arm64_defconfig
+    $ make menuconfig
+        Disable CONFIG_TFABOOT
+        Enable CONFIG_EFI_CAPSULE_AUTHENTICATE
+        Enable all configs needed for capsule update(listed above)
+    $ make all
+
+Boot the platform and perform the following steps on the U-Boot
+command line::
+
+    1. Enable capsule authentication by setting the following env
+       variable
+
+        => setenv capsule_authentication_enabled 1
+        => saveenv
+
+    2. Load the overlay dtb to memory and merge it with the base fdt
+
+        => fatload virtio 0:1 <$fdtovaddr> EFI/<ov_dtb_file>
+        => fdt addr $fdtcontroladdr
+        => fdt resize <size_of_ov_dtb_file>
+        => fdt apply <$fdtovaddr>
+
+    3. Set the following environment and UEFI boot variables
+
+        => setenv -e -nv -bs -rt -v OsIndications =0x04
+        => efidebug boot add 0 Boot0000 virtio 0:1 <capsule_file_name>
+        => efidebug boot next 0
+        => saveenv
+
+    4. Finally, the capsule update can be initiated with the following
+       command
+
+        => efidebug capsule disk-update
+
+On subsequent reboot, the platform should boot the updated U-Boot binary.
diff --git a/drivers/dfu/dfu_mtd.c b/drivers/dfu/dfu_mtd.c
index 36cd4e9..b34975d 100644
--- a/drivers/dfu/dfu_mtd.c
+++ b/drivers/dfu/dfu_mtd.c
@@ -21,7 +21,7 @@ static bool mtd_is_aligned_with_block_size(struct mtd_info *mtd, u64 size)
 static int mtd_block_op(enum dfu_op op, struct dfu_entity *dfu,
 			u64 offset, void *buf, long *len)
 {
-	u64 off, lim, remaining;
+	u64 off, lim, remaining, lock_ofs, lock_len;
 	struct mtd_info *mtd = dfu->data.mtd.info;
 	struct mtd_oob_ops io_op = {};
 	int ret = 0;
@@ -34,7 +34,7 @@ static int mtd_block_op(enum dfu_op op, struct dfu_entity *dfu,
 		return 0;
 	}
 
-	off = dfu->data.mtd.start + offset + dfu->bad_skip;
+	off = lock_ofs = dfu->data.mtd.start + offset + dfu->bad_skip;
 	lim = dfu->data.mtd.start + dfu->data.mtd.size;
 
 	if (off >= lim) {
@@ -56,12 +56,19 @@ static int mtd_block_op(enum dfu_op op, struct dfu_entity *dfu,
 	if (op == DFU_OP_WRITE) {
 		struct erase_info erase_op = {};
 
-		remaining = round_up(*len, mtd->erasesize);
+		remaining = lock_len = round_up(*len, mtd->erasesize);
 		erase_op.mtd = mtd;
 		erase_op.addr = off;
 		erase_op.len = mtd->erasesize;
 		erase_op.scrub = 0;
 
+		debug("Unlocking the mtd device\n");
+		ret = mtd_unlock(mtd, lock_ofs, lock_len);
+		if (ret && ret != -EOPNOTSUPP) {
+			printf("MTD device unlock failed\n");
+			return 0;
+		}
+
 		while (remaining) {
 			if (erase_op.addr + remaining > lim) {
 				printf("Limit reached 0x%llx while erasing at offset 0x%llx\n",
@@ -139,6 +146,13 @@ static int mtd_block_op(enum dfu_op op, struct dfu_entity *dfu,
 			io_op.len = mtd->writesize;
 	}
 
+	if (op == DFU_OP_WRITE) {
+		/* Write done, lock again */
+		debug("Locking the mtd device\n");
+		ret = mtd_lock(mtd, lock_ofs, lock_len);
+		if (ret && ret != -EOPNOTSUPP)
+			printf("MTD device lock failed\n");
+	}
 	return ret;
 }
 
diff --git a/fs/fat/fat.c b/fs/fat/fat.c
index 47344bb..157dad6 100644
--- a/fs/fat/fat.c
+++ b/fs/fat/fat.c
@@ -810,7 +810,6 @@ static void fat_itr_child(fat_itr *itr, fat_itr *parent)
  */
 void *fat_next_cluster(fat_itr *itr, unsigned int *nbytes)
 {
-	fsdata *mydata = itr->fsdata;  /* for silly macros */
 	int ret;
 	u32 sect;
 	u32 read_size;
@@ -838,8 +837,8 @@ void *fat_next_cluster(fat_itr *itr, unsigned int *nbytes)
 		read_size = itr->fsdata->clust_size;
 	}
 
-	debug("FAT read(sect=%d), clust_size=%d, read_size=%u, DIRENTSPERBLOCK=%zd\n",
-	      sect, itr->fsdata->clust_size, read_size, DIRENTSPERBLOCK);
+	log_debug("FAT read(sect=%d), clust_size=%d, read_size=%u\n",
+		  sect, itr->fsdata->clust_size, read_size);
 
 	/*
 	 * NOTE: do_fat_read_at() had complicated logic to deal w/
@@ -1378,3 +1377,21 @@ void fat_closedir(struct fs_dir_stream *dirs)
 void fat_close(void)
 {
 }
+
+int fat_uuid(char *uuid_str)
+{
+	boot_sector bs;
+	volume_info volinfo;
+	int fatsize;
+	int ret;
+	u8 *id;
+
+	ret = read_bootsectandvi(&bs, &volinfo, &fatsize);
+	if (ret)
+		return ret;
+
+	id = volinfo.volume_id;
+	sprintf(uuid_str, "%02X%02X-%02X%02X", id[3], id[2], id[1], id[0]);
+
+	return 0;
+}
diff --git a/fs/fs.c b/fs/fs.c
index 7a40206..5e80648 100644
--- a/fs/fs.c
+++ b/fs/fs.c
@@ -194,7 +194,7 @@ static struct fstype_info fstypes[] = {
 		.unlink = fs_unlink_unsupported,
 		.mkdir = fs_mkdir_unsupported,
 #endif
-		.uuid = fs_uuid_unsupported,
+		.uuid = fat_uuid,
 		.opendir = fat_opendir,
 		.readdir = fat_readdir,
 		.closedir = fat_closedir,
diff --git a/include/efi_api.h b/include/efi_api.h
index e82d4ca..ecb43a0 100644
--- a/include/efi_api.h
+++ b/include/efi_api.h
@@ -1813,6 +1813,24 @@ struct efi_variable_authentication_2 {
 } __attribute__((__packed__));
 
 /**
+ * efi_firmware_image_authentication - Capsule authentication method
+ * descriptor
+ *
+ * This structure describes an authentication information for
+ * a capsule with IMAGE_ATTRIBUTE_AUTHENTICATION_REQUIRED set
+ * and should be included as part of the capsule.
+ * Only EFI_CERT_TYPE_PKCS7_GUID is accepted.
+ *
+ * @monotonic_count: Count to prevent replay
+ * @auth_info: Authentication info
+ */
+struct efi_firmware_image_authentication {
+	uint64_t monotonic_count;
+	struct win_certificate_uefi_guid auth_info;
+} __attribute__((__packed__));
+
+
+/**
  * efi_signature_data - A format of signature
  *
  * This structure describes a single signature in signature database.
diff --git a/include/efi_loader.h b/include/efi_loader.h
index 365f3d0..4719fa9 100644
--- a/include/efi_loader.h
+++ b/include/efi_loader.h
@@ -304,15 +304,17 @@ enum efi_image_auth_status {
  * @exit_status:	exit status passed to Exit()
  * @exit_data_size:	exit data size passed to Exit()
  * @exit_data:		exit data passed to Exit()
- * @exit_jmp:		long jump buffer for returning form started image
+ * @exit_jmp:		long jump buffer for returning from started image
  * @entry:		entry address of the relocated image
+ * @image_type:		indicates if the image is an applicition or a driver
+ * @auth_status:	indicates if the image is authenticated
  */
 struct efi_loaded_image_obj {
 	struct efi_object header;
-	efi_status_t exit_status;
+	efi_status_t *exit_status;
 	efi_uintn_t *exit_data_size;
 	u16 **exit_data;
-	struct jmp_buf_data exit_jmp;
+	struct jmp_buf_data *exit_jmp;
 	EFIAPI efi_status_t (*entry)(efi_handle_t image_handle,
 				     struct efi_system_table *st);
 	u16 image_type;
@@ -811,18 +813,27 @@ efi_status_t efi_image_region_add(struct efi_image_regions *regs,
 				  int nocheck);
 
 void efi_sigstore_free(struct efi_signature_store *sigstore);
+struct efi_signature_store *efi_build_signature_store(void *sig_list,
+						      efi_uintn_t size);
 struct efi_signature_store *efi_sigstore_parse_sigdb(u16 *name);
 
 bool efi_secure_boot_enabled(void);
 
+bool efi_capsule_auth_enabled(void);
+
 bool efi_image_parse(void *efi, size_t len, struct efi_image_regions **regp,
 		     WIN_CERTIFICATE **auth, size_t *auth_len);
 
+struct pkcs7_message *efi_parse_pkcs7_header(const void *buf,
+					     size_t buflen,
+					     u8 **tmpbuf);
+
 /* runtime implementation of memcpy() */
 void efi_memcpy_runtime(void *dest, const void *src, size_t n);
 
 /* commonly used helper function */
-u16 *efi_create_indexed_name(u16 *buffer, const char *name, unsigned int index);
+u16 *efi_create_indexed_name(u16 *buffer, size_t buffer_size, const char *name,
+			     unsigned int index);
 
 extern const struct efi_firmware_management_protocol efi_fmp_fit;
 extern const struct efi_firmware_management_protocol efi_fmp_raw;
@@ -838,6 +849,10 @@ efi_status_t EFIAPI efi_query_capsule_caps(
 		u64 *maximum_capsule_size,
 		u32 *reset_type);
 
+efi_status_t efi_capsule_authenticate(const void *capsule,
+				      efi_uintn_t capsule_size,
+				      void **image, efi_uintn_t *image_size);
+
 #define EFI_CAPSULE_DIR L"\\EFI\\UpdateCapsule\\"
 
 /* Hook at initialization */
diff --git a/include/efi_variable.h b/include/efi_variable.h
index 4704a3c..bf50762 100644
--- a/include/efi_variable.h
+++ b/include/efi_variable.h
@@ -91,7 +91,7 @@ efi_status_t efi_query_variable_info_int(u32 attributes,
 
 #define EFI_VAR_FILE_NAME "ubootefi.var"
 
-#define EFI_VAR_BUF_SIZE 0x4000
+#define EFI_VAR_BUF_SIZE CONFIG_EFI_VAR_BUF_SIZE
 
 /*
  * This constant identifies the file format for storing UEFI variables in
diff --git a/include/fat.h b/include/fat.h
index 3c29a44..b9f273f 100644
--- a/include/fat.h
+++ b/include/fat.h
@@ -22,7 +22,6 @@ struct disk_partition;
 
 #define MAX_CLUSTSIZE	CONFIG_FS_FAT_MAX_CLUSTSIZE
 
-#define DIRENTSPERBLOCK	(mydata->sect_size / sizeof(dir_entry))
 #define DIRENTSPERCLUST	((mydata->clust_size * mydata->sect_size) / \
 			 sizeof(dir_entry))
 
@@ -213,4 +212,16 @@ int fat_unlink(const char *filename);
 int fat_mkdir(const char *dirname);
 void fat_close(void);
 void *fat_next_cluster(fat_itr *itr, unsigned int *nbytes);
+
+/**
+ * fat_uuid() - get FAT volume ID
+ *
+ * The FAT volume ID returned in @uuid_str as hexadecimal number in XXXX-XXXX
+ * format.
+ *
+ * @uuid_str:	caller allocated buffer of at least 10 bytes for the volume ID
+ * Return:	0 on success
+ */
+int fat_uuid(char *uuid_str);
+
 #endif /* _FAT_H_ */
diff --git a/arch/x86/include/asm/fsp/fsp_types.h b/include/signatures.h
index 3d5b17e..4042db1 100644
--- a/arch/x86/include/asm/fsp/fsp_types.h
+++ b/include/signatures.h
@@ -4,8 +4,8 @@
  * Copyright (C) 2014, Bin Meng <bmeng.cn@gmail.com>
  */
 
-#ifndef __FSP_TYPES_H__
-#define __FSP_TYPES_H__
+#ifndef __SIGNATURES_H__
+#define __SIGNATURES_H__
 
 /**
  * Returns a 16-bit signature built from 2 ASCII characters.
@@ -59,4 +59,4 @@
 #define SIGNATURE_64(A, B, C, D, E, F, G, H)	\
 	(SIGNATURE_32(A, B, C, D) | ((u64)(SIGNATURE_32(E, F, G, H)) << 32))
 
-#endif
+#endif /* __SIGNATURES_H__ */
diff --git a/lib/crypto/pkcs7_verify.c b/lib/crypto/pkcs7_verify.c
index 320ba49..58683ef 100644
--- a/lib/crypto/pkcs7_verify.c
+++ b/lib/crypto/pkcs7_verify.c
@@ -50,8 +50,15 @@ static int pkcs7_digest(struct pkcs7_message *pkcs7,
 	struct image_region regions[2];
 	int ret = 0;
 
-	/* The digest was calculated already. */
-	if (sig->digest)
+	/*
+	 * [RFC2315 9.3]
+	 * If the authenticated attributes are present,
+	 * the message-digest is calculated on the
+	 * attributes present in the
+	 * authenticatedAttributes field and not just
+	 * the contents field
+	 */
+	if (!sinfo->authattrs && sig->digest)
 		return 0;
 
 	if (!sinfo->sig->hash_algo)
@@ -63,17 +70,25 @@ static int pkcs7_digest(struct pkcs7_message *pkcs7,
 	else
 		return -ENOPKG;
 
-	sig->digest = calloc(1, sig->digest_size);
-	if (!sig->digest) {
-		pr_warn("Sig %u: Out of memory\n", sinfo->index);
-		return -ENOMEM;
-	}
+	/*
+	 * Calculate the hash only if the data is present.
+	 * In case of authenticated variable and capsule,
+	 * the hash has already been calculated on the
+	 * efi_image_regions and populated
+	 */
+	if (pkcs7->data) {
+		sig->digest = calloc(1, sig->digest_size);
+		if (!sig->digest) {
+			pr_warn("Sig %u: Out of memory\n", sinfo->index);
+			return -ENOMEM;
+		}
 
-	regions[0].data = pkcs7->data;
-	regions[0].size = pkcs7->data_len;
+		regions[0].data = pkcs7->data;
+		regions[0].size = pkcs7->data_len;
 
-	/* Digest the message [RFC2315 9.3] */
-	hash_calculate(sinfo->sig->hash_algo, regions, 1, sig->digest);
+		/* Digest the message [RFC2315 9.3] */
+		hash_calculate(sinfo->sig->hash_algo, regions, 1, sig->digest);
+	}
 
 	/* However, if there are authenticated attributes, there must be a
 	 * message digest attribute amongst them which corresponds to the
diff --git a/lib/efi_loader/Kconfig b/lib/efi_loader/Kconfig
index 073d90c..fdf245d 100644
--- a/lib/efi_loader/Kconfig
+++ b/lib/efi_loader/Kconfig
@@ -77,6 +77,20 @@ config EFI_VAR_SEED_FILE
 
 endif
 
+config EFI_VAR_BUF_SIZE
+	int "Memory size of the UEFI variable store"
+	default 16384
+	range 4096 2147483647
+	help
+	  This defines the size in bytes of the memory area reserved for keeping
+	  UEFI variables.
+
+	  When using StandAloneMM (CONFIG_EFI_MM_COMM_TEE=y) this value should
+	  match the value of PcdFlashNvStorageVariableSize used to compile the
+	  StandAloneMM module.
+
+	  Minimum 4096, default 16384.
+
 config EFI_GET_TIME
 	bool "GetTime() runtime service"
 	depends on DM_RTC
@@ -139,6 +153,23 @@ config EFI_CAPSULE_FIRMWARE_MANAGEMENT
 	  Select this option if you want to enable capsule-based
 	  firmware update using Firmware Management Protocol.
 
+config EFI_CAPSULE_AUTHENTICATE
+	bool "Update Capsule authentication"
+	depends on EFI_CAPSULE_FIRMWARE
+	depends on EFI_CAPSULE_ON_DISK
+	depends on EFI_CAPSULE_FIRMWARE_MANAGEMENT
+	select SHA256
+	select RSA
+	select RSA_VERIFY
+	select RSA_VERIFY_WITH_PKEY
+	select X509_CERTIFICATE_PARSER
+	select PKCS7_MESSAGE_PARSER
+	select PKCS7_VERIFY
+	default n
+	help
+	  Select this option if you want to enable capsule
+	  authentication
+
 config EFI_CAPSULE_FIRMWARE_FIT
 	bool "FMP driver for FIT image"
 	depends on EFI_CAPSULE_FIRMWARE_MANAGEMENT
diff --git a/lib/efi_loader/efi_bootmgr.c b/lib/efi_loader/efi_bootmgr.c
index 61dc72a..d3be2f9 100644
--- a/lib/efi_loader/efi_bootmgr.c
+++ b/lib/efi_loader/efi_bootmgr.c
@@ -275,7 +275,7 @@ static efi_status_t try_load_entry(u16 n, efi_handle_t *handle,
 		memcpy(*load_options, lo.optional_data, size);
 		ret = efi_set_load_options(*handle, size, *load_options);
 	} else {
-		load_options = NULL;
+		*load_options = NULL;
 	}
 
 error:
diff --git a/lib/efi_loader/efi_boottime.c b/lib/efi_loader/efi_boottime.c
index 03053e8..b2cb016 100644
--- a/lib/efi_loader/efi_boottime.c
+++ b/lib/efi_loader/efi_boottime.c
@@ -247,8 +247,8 @@ static void efi_queue_event(struct efi_event *event)
 		}
 		if (event)
 			list_add_tail(&event->queue_link, &efi_event_queue);
+		efi_process_event_queue();
 	}
-	efi_process_event_queue();
 }
 
 /**
@@ -274,8 +274,8 @@ efi_status_t is_valid_tpl(efi_uintn_t tpl)
  * efi_signal_event() - signal an EFI event
  * @event:     event to signal
  *
- * This function signals an event. If the event belongs to an event group all
- * events of the group are signaled. If they are of type EVT_NOTIFY_SIGNAL
+ * This function signals an event. If the event belongs to an event group, all
+ * events of the group are signaled. If they are of type EVT_NOTIFY_SIGNAL,
  * their notification function is queued.
  *
  * For the SignalEvent service see efi_signal_event_ext.
@@ -2161,7 +2161,7 @@ static efi_status_t EFIAPI efi_exit_boot_services(efi_handle_t image_handle,
 	}
 
 	if (!efi_st_keep_devices) {
-		if IS_ENABLED(CONFIG_USB_DEVICE)
+		if (IS_ENABLED(CONFIG_USB_DEVICE))
 			udc_disconnect();
 		board_quiesce_devices();
 		dm_remove_devices_flags(DM_REMOVE_ACTIVE_ALL);
@@ -2978,6 +2978,8 @@ efi_status_t EFIAPI efi_start_image(efi_handle_t image_handle,
 	efi_status_t ret;
 	void *info;
 	efi_handle_t parent_image = current_image;
+	efi_status_t exit_status;
+	struct jmp_buf_data exit_jmp;
 
 	EFI_ENTRY("%p, %p, %p", image_handle, exit_data_size, exit_data);
 
@@ -2999,9 +3001,11 @@ efi_status_t EFIAPI efi_start_image(efi_handle_t image_handle,
 
 	image_obj->exit_data_size = exit_data_size;
 	image_obj->exit_data = exit_data;
+	image_obj->exit_status = &exit_status;
+	image_obj->exit_jmp = &exit_jmp;
 
 	/* call the image! */
-	if (setjmp(&image_obj->exit_jmp)) {
+	if (setjmp(&exit_jmp)) {
 		/*
 		 * We called the entry point of the child image with EFI_CALL
 		 * in the lines below. The child image called the Exit() boot
@@ -3023,10 +3027,10 @@ efi_status_t EFIAPI efi_start_image(efi_handle_t image_handle,
 		 */
 		assert(__efi_entry_check());
 		EFI_PRINT("%lu returned by started image\n",
-			  (unsigned long)((uintptr_t)image_obj->exit_status &
+			  (unsigned long)((uintptr_t)exit_status &
 			  ~EFI_ERROR_MASK));
 		current_image = parent_image;
-		return EFI_EXIT(image_obj->exit_status);
+		return EFI_EXIT(exit_status);
 	}
 
 	current_image = image_handle;
@@ -3209,6 +3213,7 @@ static efi_status_t EFIAPI efi_exit(efi_handle_t image_handle,
 	struct efi_loaded_image *loaded_image_protocol;
 	struct efi_loaded_image_obj *image_obj =
 		(struct efi_loaded_image_obj *)image_handle;
+	struct jmp_buf_data *exit_jmp;
 
 	EFI_ENTRY("%p, %ld, %zu, %p", image_handle, exit_status,
 		  exit_data_size, exit_data);
@@ -3250,6 +3255,9 @@ static efi_status_t EFIAPI efi_exit(efi_handle_t image_handle,
 		if (ret != EFI_SUCCESS)
 			EFI_PRINT("%s: out of memory\n", __func__);
 	}
+	/* efi_delete_image() frees image_obj. Copy before the call. */
+	exit_jmp = image_obj->exit_jmp;
+	*image_obj->exit_status = exit_status;
 	if (image_obj->image_type == IMAGE_SUBSYSTEM_EFI_APPLICATION ||
 	    exit_status != EFI_SUCCESS)
 		efi_delete_image(image_obj, loaded_image_protocol);
@@ -3263,8 +3271,7 @@ static efi_status_t EFIAPI efi_exit(efi_handle_t image_handle,
 	 */
 	efi_restore_gd();
 
-	image_obj->exit_status = exit_status;
-	longjmp(&image_obj->exit_jmp, 1);
+	longjmp(exit_jmp, 1);
 
 	panic("EFI application exited");
 out:
diff --git a/lib/efi_loader/efi_capsule.c b/lib/efi_loader/efi_capsule.c
index ea22ee7..dad1b0f 100644
--- a/lib/efi_loader/efi_capsule.c
+++ b/lib/efi_loader/efi_capsule.c
@@ -14,6 +14,10 @@
 #include <mapmem.h>
 #include <sort.h>
 
+#include <crypto/pkcs7.h>
+#include <crypto/pkcs7_parser.h>
+#include <linux/err.h>
+
 const efi_guid_t efi_guid_capsule_report = EFI_CAPSULE_REPORT_GUID;
 static const efi_guid_t efi_guid_firmware_management_capsule_id =
 		EFI_FIRMWARE_MANAGEMENT_CAPSULE_ID_GUID;
@@ -73,8 +77,8 @@ void set_capsule_result(int index, struct efi_capsule_header *capsule,
 	struct efi_time time;
 	efi_status_t ret;
 
-	efi_create_indexed_name(variable_name16, "Capsule", index);
-
+	efi_create_indexed_name(variable_name16, sizeof(variable_name16),
+				"Capsule", index);
 	result.variable_total_size = sizeof(result);
 	result.capsule_guid = capsule->capsule_guid;
 	ret = EFI_CALL((*efi_runtime_services.get_time)(&time, NULL));
@@ -191,6 +195,124 @@ skip:
 	return NULL;
 }
 
+#if defined(CONFIG_EFI_CAPSULE_AUTHENTICATE)
+
+const efi_guid_t efi_guid_capsule_root_cert_guid =
+	EFI_FIRMWARE_MANAGEMENT_CAPSULE_ID_GUID;
+
+__weak int efi_get_public_key_data(void **pkey, efi_uintn_t *pkey_len)
+{
+	/* The platform is supposed to provide
+	 * a method for getting the public key
+	 * stored in the form of efi signature
+	 * list
+	 */
+	return 0;
+}
+
+efi_status_t efi_capsule_authenticate(const void *capsule, efi_uintn_t capsule_size,
+				      void **image, efi_uintn_t *image_size)
+{
+	u8 *buf;
+	int ret;
+	void *fdt_pkey, *pkey;
+	efi_uintn_t pkey_len;
+	uint64_t monotonic_count;
+	struct efi_signature_store *truststore;
+	struct pkcs7_message *capsule_sig;
+	struct efi_image_regions *regs;
+	struct efi_firmware_image_authentication *auth_hdr;
+	efi_status_t status;
+
+	status = EFI_SECURITY_VIOLATION;
+	capsule_sig = NULL;
+	truststore = NULL;
+	regs = NULL;
+
+	/* Sanity checks */
+	if (capsule == NULL || capsule_size == 0)
+		goto out;
+
+	auth_hdr = (struct efi_firmware_image_authentication *)capsule;
+	if (capsule_size < sizeof(*auth_hdr))
+		goto out;
+
+	if (auth_hdr->auth_info.hdr.dwLength <=
+	    offsetof(struct win_certificate_uefi_guid, cert_data))
+		goto out;
+
+	if (guidcmp(&auth_hdr->auth_info.cert_type, &efi_guid_cert_type_pkcs7))
+		goto out;
+
+	*image = (uint8_t *)capsule + sizeof(auth_hdr->monotonic_count) +
+		auth_hdr->auth_info.hdr.dwLength;
+	*image_size = capsule_size - auth_hdr->auth_info.hdr.dwLength -
+		sizeof(auth_hdr->monotonic_count);
+	memcpy(&monotonic_count, &auth_hdr->monotonic_count,
+	       sizeof(monotonic_count));
+
+	/* data to be digested */
+	regs = calloc(sizeof(*regs) + sizeof(struct image_region) * 2, 1);
+	if (!regs)
+		goto out;
+
+	regs->max = 2;
+	efi_image_region_add(regs, (uint8_t *)*image,
+			     (uint8_t *)*image + *image_size, 1);
+
+	efi_image_region_add(regs, (uint8_t *)&monotonic_count,
+			     (uint8_t *)&monotonic_count + sizeof(monotonic_count),
+			     1);
+
+	capsule_sig = efi_parse_pkcs7_header(auth_hdr->auth_info.cert_data,
+					     auth_hdr->auth_info.hdr.dwLength
+					     - sizeof(auth_hdr->auth_info),
+					     &buf);
+	if (IS_ERR(capsule_sig)) {
+		debug("Parsing variable's pkcs7 header failed\n");
+		capsule_sig = NULL;
+		goto out;
+	}
+
+	ret = efi_get_public_key_data(&fdt_pkey, &pkey_len);
+	if (ret < 0)
+		goto out;
+
+	pkey = malloc(pkey_len);
+	if (!pkey)
+		goto out;
+
+	memcpy(pkey, fdt_pkey, pkey_len);
+	truststore = efi_build_signature_store(pkey, pkey_len);
+	if (!truststore)
+		goto out;
+
+	/* verify signature */
+	if (efi_signature_verify(regs, capsule_sig, truststore, NULL)) {
+		debug("Verified\n");
+	} else {
+		debug("Verifying variable's signature failed\n");
+		goto out;
+	}
+
+	status = EFI_SUCCESS;
+
+out:
+	efi_sigstore_free(truststore);
+	pkcs7_free_message(capsule_sig);
+	free(regs);
+
+	return status;
+}
+#else
+efi_status_t efi_capsule_authenticate(const void *capsule, efi_uintn_t capsule_size,
+				      void **image, efi_uintn_t *image_size)
+{
+	return EFI_UNSUPPORTED;
+}
+#endif /* CONFIG_EFI_CAPSULE_AUTHENTICATE */
+
+
 /**
  * efi_capsule_update_firmware - update firmware from capsule
  * @capsule_data:	Capsule
@@ -896,7 +1018,8 @@ efi_status_t efi_launch_capsules(void)
 	free(files);
 
 	/* CapsuleLast */
-	efi_create_indexed_name(variable_name16, "Capsule", index - 1);
+	efi_create_indexed_name(variable_name16, sizeof(variable_name16),
+				"Capsule", index - 1);
 	efi_set_variable_int(L"CapsuleLast", &efi_guid_capsule_report,
 			     EFI_VARIABLE_READ_ONLY |
 			     EFI_VARIABLE_NON_VOLATILE |
diff --git a/lib/efi_loader/efi_console.c b/lib/efi_loader/efi_console.c
index 011acca..7051095 100644
--- a/lib/efi_loader/efi_console.c
+++ b/lib/efi_loader/efi_console.c
@@ -14,6 +14,7 @@
 #include <env.h>
 #include <stdio_dev.h>
 #include <video_console.h>
+#include <linux/delay.h>
 
 #define EFI_COUT_MODE_2 2
 #define EFI_MAX_COUT_MODE 3
@@ -689,6 +690,17 @@ static efi_status_t efi_cin_read_key(struct efi_key_data *key)
 	switch (ch) {
 	case 0x1b:
 		/*
+		 * If a second key is received within 10 ms, assume that we are
+		 * dealing with an escape sequence. Otherwise consider this the
+		 * escape key being hit. 10 ms is long enough to work fine at
+		 * 1200 baud and above.
+		 */
+		udelay(10000);
+		if (!tstc()) {
+			pressed_key.scan_code = 23;
+			break;
+		}
+		/*
 		 * Xterm Control Sequences
 		 * https://www.xfree86.org/4.8.0/ctlseqs.html
 		 */
diff --git a/lib/efi_loader/efi_firmware.c b/lib/efi_loader/efi_firmware.c
index 72c560d..5e401bb 100644
--- a/lib/efi_loader/efi_firmware.c
+++ b/lib/efi_loader/efi_firmware.c
@@ -11,8 +11,30 @@
 #include <dfu.h>
 #include <efi_loader.h>
 #include <image.h>
+#include <signatures.h>
+
 #include <linux/list.h>
 
+#define FMP_PAYLOAD_HDR_SIGNATURE	SIGNATURE_32('M', 'S', 'S', '1')
+
+/**
+ * struct fmp_payload_header - EDK2 header for the FMP payload
+ *
+ * This structure describes the header which is preprended to the
+ * FMP payload by the edk2 capsule generation scripts.
+ *
+ * @signature:			Header signature used to identify the header
+ * @header_size:		Size of the structure
+ * @fw_version:			Firmware versions used
+ * @lowest_supported_version:	Lowest supported version
+ */
+struct fmp_payload_header {
+	u32 signature;
+	u32 header_size;
+	u32 fw_version;
+	u32 lowest_supported_version;
+};
+
 /* Place holder; not supported */
 static
 efi_status_t EFIAPI efi_firmware_get_image_unsupported(
@@ -162,9 +184,16 @@ static efi_status_t efi_get_dfu_info(
 		image_info[i].version_name = NULL; /* not supported */
 		image_info[i].size = 0;
 		image_info[i].attributes_supported =
-				IMAGE_ATTRIBUTE_IMAGE_UPDATABLE;
+			IMAGE_ATTRIBUTE_IMAGE_UPDATABLE |
+			IMAGE_ATTRIBUTE_AUTHENTICATION_REQUIRED;
 		image_info[i].attributes_setting =
 				IMAGE_ATTRIBUTE_IMAGE_UPDATABLE;
+
+		/* Check if the capsule authentication is enabled */
+		if (env_get("capsule_authentication_enabled"))
+			image_info[0].attributes_setting |=
+				IMAGE_ATTRIBUTE_AUTHENTICATION_REQUIRED;
+
 		image_info[i].lowest_supported_image_version = 0;
 		image_info[i].last_attempt_version = 0;
 		image_info[i].last_attempt_status = LAST_ATTEMPT_STATUS_SUCCESS;
@@ -379,12 +408,58 @@ efi_status_t EFIAPI efi_firmware_raw_set_image(
 	efi_status_t (*progress)(efi_uintn_t completion),
 	u16 **abort_reason)
 {
+	u32 fmp_hdr_signature;
+	struct fmp_payload_header *header;
+	void *capsule_payload;
+	efi_status_t status;
+	efi_uintn_t capsule_payload_size;
+
 	EFI_ENTRY("%p %d %p %ld %p %p %p\n", this, image_index, image,
 		  image_size, vendor_code, progress, abort_reason);
 
 	if (!image)
 		return EFI_EXIT(EFI_INVALID_PARAMETER);
 
+	/* Authenticate the capsule if authentication enabled */
+	if (IS_ENABLED(CONFIG_EFI_CAPSULE_AUTHENTICATE) &&
+	    env_get("capsule_authentication_enabled")) {
+		capsule_payload = NULL;
+		capsule_payload_size = 0;
+		status = efi_capsule_authenticate(image, image_size,
+						  &capsule_payload,
+						  &capsule_payload_size);
+
+		if (status == EFI_SECURITY_VIOLATION) {
+			printf("Capsule authentication check failed. Aborting update\n");
+			return EFI_EXIT(status);
+		} else if (status != EFI_SUCCESS) {
+			return EFI_EXIT(status);
+		}
+
+		debug("Capsule authentication successfull\n");
+		image = capsule_payload;
+		image_size = capsule_payload_size;
+	} else {
+		debug("Capsule authentication disabled. ");
+		debug("Updating capsule without authenticating.\n");
+	}
+
+	fmp_hdr_signature = FMP_PAYLOAD_HDR_SIGNATURE;
+	header = (void *)image;
+
+	if (!memcmp(&header->signature, &fmp_hdr_signature,
+		    sizeof(fmp_hdr_signature))) {
+		/*
+		 * When building the capsule with the scripts in
+		 * edk2, a FMP header is inserted above the capsule
+		 * payload. Compensate for this header to get the
+		 * actual payload that is to be updated.
+		 */
+		image += header->header_size;
+		image_size -= header->header_size;
+
+	}
+
 	if (dfu_write_by_alt(image_index - 1, (void *)image, image_size,
 			     NULL, NULL))
 		return EFI_EXIT(EFI_DEVICE_ERROR);
diff --git a/lib/efi_loader/efi_load_initrd.c b/lib/efi_loader/efi_load_initrd.c
index 4bf3b5e..b9ee883 100644
--- a/lib/efi_loader/efi_load_initrd.c
+++ b/lib/efi_loader/efi_load_initrd.c
@@ -4,13 +4,11 @@
  */
 
 #include <common.h>
-#include <env.h>
-#include <malloc.h>
-#include <mapmem.h>
-#include <dm.h>
-#include <fs.h>
 #include <efi_loader.h>
 #include <efi_load_initrd.h>
+#include <fs.h>
+#include <malloc.h>
+#include <mapmem.h>
 
 static efi_status_t EFIAPI
 efi_load_file2_initrd(struct efi_load_file_protocol *this,
diff --git a/lib/efi_loader/efi_setup.c b/lib/efi_loader/efi_setup.c
index ce6292f..5800cbf 100644
--- a/lib/efi_loader/efi_setup.c
+++ b/lib/efi_loader/efi_setup.c
@@ -257,11 +257,6 @@ efi_status_t efi_init_obj_list(void)
 	if (ret != EFI_SUCCESS)
 		goto out;
 #endif
-#ifdef CONFIG_EFI_LOAD_FILE2_INITRD
-	ret = efi_initrd_register();
-	if (ret != EFI_SUCCESS)
-		goto out;
-#endif
 #ifdef CONFIG_NET
 	ret = efi_net_register();
 	if (ret != EFI_SUCCESS)
diff --git a/lib/efi_loader/efi_signature.c b/lib/efi_loader/efi_signature.c
index 79dee27..c7ec275 100644
--- a/lib/efi_loader/efi_signature.c
+++ b/lib/efi_loader/efi_signature.c
@@ -26,7 +26,92 @@ const efi_guid_t efi_guid_cert_x509 = EFI_CERT_X509_GUID;
 const efi_guid_t efi_guid_cert_x509_sha256 = EFI_CERT_X509_SHA256_GUID;
 const efi_guid_t efi_guid_cert_type_pkcs7 = EFI_CERT_TYPE_PKCS7_GUID;
 
-#ifdef CONFIG_EFI_SECURE_BOOT
+#if defined(CONFIG_EFI_SECURE_BOOT) || defined(CONFIG_EFI_CAPSULE_AUTHENTICATE)
+static u8 pkcs7_hdr[] = {
+	/* SEQUENCE */
+	0x30, 0x82, 0x05, 0xc7,
+	/* OID: pkcs7-signedData */
+	0x06, 0x09, 0x2a, 0x86, 0x48, 0x86, 0xf7, 0x0d, 0x01, 0x07, 0x02,
+	/* Context Structured? */
+	0xa0, 0x82, 0x05, 0xb8,
+};
+
+/**
+ * efi_parse_pkcs7_header - parse a signature in payload
+ * @buf:	Pointer to payload's value
+ * @buflen:	Length of @buf
+ * @tmpbuf:	Pointer to temporary buffer
+ *
+ * Parse a signature embedded in payload's value and instantiate
+ * a pkcs7_message structure. Since pkcs7_parse_message() accepts only
+ * pkcs7's signedData, some header needed be prepended for correctly
+ * parsing authentication data
+ * A temporary buffer will be allocated if needed, and it should be
+ * kept valid during the authentication because some data in the buffer
+ * will be referenced by efi_signature_verify().
+ *
+ * Return:	Pointer to pkcs7_message structure on success, NULL on error
+ */
+struct pkcs7_message *efi_parse_pkcs7_header(const void *buf,
+					     size_t buflen,
+					     u8 **tmpbuf)
+{
+	u8 *ebuf;
+	size_t ebuflen, len;
+	struct pkcs7_message *msg;
+
+	/*
+	 * This is the best assumption to check if the binary is
+	 * already in a form of pkcs7's signedData.
+	 */
+	if (buflen > sizeof(pkcs7_hdr) &&
+	    !memcmp(&((u8 *)buf)[4], &pkcs7_hdr[4], 11)) {
+		msg = pkcs7_parse_message(buf, buflen);
+		if (IS_ERR(msg))
+			return NULL;
+		return msg;
+	}
+
+	/*
+	 * Otherwise, we should add a dummy prefix sequence for pkcs7
+	 * message parser to be able to process.
+	 * NOTE: EDK2 also uses similar hack in WrapPkcs7Data()
+	 * in CryptoPkg/Library/BaseCryptLib/Pk/CryptPkcs7VerifyCommon.c
+	 * TODO:
+	 * The header should be composed in a more refined manner.
+	 */
+	EFI_PRINT("Makeshift prefix added to authentication data\n");
+	ebuflen = sizeof(pkcs7_hdr) + buflen;
+	if (ebuflen <= 0x7f) {
+		EFI_PRINT("Data is too short\n");
+		return NULL;
+	}
+
+	ebuf = malloc(ebuflen);
+	if (!ebuf) {
+		EFI_PRINT("Out of memory\n");
+		return NULL;
+	}
+
+	memcpy(ebuf, pkcs7_hdr, sizeof(pkcs7_hdr));
+	memcpy(ebuf + sizeof(pkcs7_hdr), buf, buflen);
+	len = ebuflen - 4;
+	ebuf[2] = (len >> 8) & 0xff;
+	ebuf[3] = len & 0xff;
+	len = ebuflen - 0x13;
+	ebuf[0x11] = (len >> 8) & 0xff;
+	ebuf[0x12] = len & 0xff;
+
+	msg = pkcs7_parse_message(ebuf, ebuflen);
+
+	if (IS_ERR(msg)) {
+		free(ebuf);
+		return NULL;
+	}
+
+	*tmpbuf = ebuf;
+	return msg;
+}
 
 /**
  * efi_hash_regions - calculate a hash value
@@ -652,6 +737,63 @@ err:
 }
 
 /**
+ * efi_sigstore_parse_sigdb - parse the signature list and populate
+ * the signature store
+ *
+ * @sig_list:	Pointer to the signature list
+ * @size:	Size of the signature list
+ *
+ * Parse the efi signature list and instantiate a signature store
+ * structure.
+ *
+ * Return:	Pointer to signature store on success, NULL on error
+ */
+struct efi_signature_store *efi_build_signature_store(void *sig_list,
+						      efi_uintn_t size)
+{
+	struct efi_signature_list *esl;
+	struct efi_signature_store *sigstore = NULL, *siglist;
+
+	esl = sig_list;
+	while (size > 0) {
+		/* List must exist if there is remaining data. */
+		if (size < sizeof(*esl)) {
+			EFI_PRINT("Signature list in wrong format\n");
+			goto err;
+		}
+
+		if (size < esl->signature_list_size) {
+			EFI_PRINT("Signature list in wrong format\n");
+			goto err;
+		}
+
+		/* Parse a single siglist. */
+		siglist = efi_sigstore_parse_siglist(esl);
+		if (!siglist) {
+			EFI_PRINT("Parsing of signature list of failed\n");
+			goto err;
+		}
+
+		/* Append siglist */
+		siglist->next = sigstore;
+		sigstore = siglist;
+
+		/* Next */
+		size -= esl->signature_list_size;
+		esl = (void *)esl + esl->signature_list_size;
+	}
+	free(sig_list);
+
+	return sigstore;
+
+err:
+	efi_sigstore_free(sigstore);
+	free(sig_list);
+
+	return NULL;
+}
+
+/**
  * efi_sigstore_parse_sigdb - parse a signature database variable
  * @name:	Variable's name
  *
@@ -662,8 +804,7 @@ err:
  */
 struct efi_signature_store *efi_sigstore_parse_sigdb(u16 *name)
 {
-	struct efi_signature_store *sigstore = NULL, *siglist;
-	struct efi_signature_list *esl;
+	struct efi_signature_store *sigstore = NULL;
 	const efi_guid_t *vendor;
 	void *db;
 	efi_uintn_t db_size;
@@ -699,47 +840,10 @@ struct efi_signature_store *efi_sigstore_parse_sigdb(u16 *name)
 	ret = EFI_CALL(efi_get_variable(name, vendor, NULL, &db_size, db));
 	if (ret != EFI_SUCCESS) {
 		EFI_PRINT("Getting variable, %ls, failed\n", name);
-		goto err;
-	}
-
-	/* Parse siglist list */
-	esl = db;
-	while (db_size > 0) {
-		/* List must exist if there is remaining data. */
-		if (db_size < sizeof(*esl)) {
-			EFI_PRINT("variable, %ls, in wrong format\n", name);
-			goto err;
-		}
-
-		if (db_size < esl->signature_list_size) {
-			EFI_PRINT("variable, %ls, in wrong format\n", name);
-			goto err;
-		}
-
-		/* Parse a single siglist. */
-		siglist = efi_sigstore_parse_siglist(esl);
-		if (!siglist) {
-			EFI_PRINT("Parsing signature list of %ls failed\n",
-				  name);
-			goto err;
-		}
-
-		/* Append siglist */
-		siglist->next = sigstore;
-		sigstore = siglist;
-
-		/* Next */
-		db_size -= esl->signature_list_size;
-		esl = (void *)esl + esl->signature_list_size;
+		free(db);
+		return NULL;
 	}
-	free(db);
 
-	return sigstore;
-
-err:
-	efi_sigstore_free(sigstore);
-	free(db);
-
-	return NULL;
+	return efi_build_signature_store(db, db_size);
 }
-#endif /* CONFIG_EFI_SECURE_BOOT */
+#endif /* CONFIG_EFI_SECURE_BOOT || CONFIG_EFI_CAPSULE_AUTHENTICATE */
diff --git a/lib/efi_loader/efi_string.c b/lib/efi_loader/efi_string.c
index 3de721f..9627242 100644
--- a/lib/efi_loader/efi_string.c
+++ b/lib/efi_loader/efi_string.c
@@ -23,13 +23,19 @@
  * Return: A pointer to the next position after the created string
  *	   in @buffer, or NULL otherwise
  */
-u16 *efi_create_indexed_name(u16 *buffer, const char *name, unsigned int index)
+u16 *efi_create_indexed_name(u16 *buffer, size_t buffer_size, const char *name,
+			     unsigned int index)
 {
 	u16 *p = buffer;
 	char index_buf[5];
+	size_t size;
 
+	size = (utf8_utf16_strlen(name) * sizeof(u16) +
+		sizeof(index_buf) * sizeof(u16));
+	if (buffer_size < size)
+		return NULL;
 	utf8_utf16_strcpy(&p, name);
-	sprintf(index_buf, "%04X", index);
+	snprintf(index_buf, sizeof(index_buf), "%04X", index);
 	utf8_utf16_strcpy(&p, index_buf);
 
 	return p;
diff --git a/lib/efi_loader/efi_variable.c b/lib/efi_loader/efi_variable.c
index 0c689cf..ba0874e 100644
--- a/lib/efi_loader/efi_variable.c
+++ b/lib/efi_loader/efi_variable.c
@@ -24,91 +24,6 @@
 #include <asm/sections.h>
 
 #ifdef CONFIG_EFI_SECURE_BOOT
-static u8 pkcs7_hdr[] = {
-	/* SEQUENCE */
-	0x30, 0x82, 0x05, 0xc7,
-	/* OID: pkcs7-signedData */
-	0x06, 0x09, 0x2a, 0x86, 0x48, 0x86, 0xf7, 0x0d, 0x01, 0x07, 0x02,
-	/* Context Structured? */
-	0xa0, 0x82, 0x05, 0xb8,
-};
-
-/**
- * efi_variable_parse_signature - parse a signature in variable
- * @buf:	Pointer to variable's value
- * @buflen:	Length of @buf
- * @tmpbuf:	Pointer to temporary buffer
- *
- * Parse a signature embedded in variable's value and instantiate
- * a pkcs7_message structure. Since pkcs7_parse_message() accepts only
- * pkcs7's signedData, some header needed be prepended for correctly
- * parsing authentication data, particularly for variable's.
- * A temporary buffer will be allocated if needed, and it should be
- * kept valid during the authentication because some data in the buffer
- * will be referenced by efi_signature_verify().
- *
- * Return:	Pointer to pkcs7_message structure on success, NULL on error
- */
-static struct pkcs7_message *efi_variable_parse_signature(const void *buf,
-							  size_t buflen,
-							  u8 **tmpbuf)
-{
-	u8 *ebuf;
-	size_t ebuflen, len;
-	struct pkcs7_message *msg;
-
-	/*
-	 * This is the best assumption to check if the binary is
-	 * already in a form of pkcs7's signedData.
-	 */
-	if (buflen > sizeof(pkcs7_hdr) &&
-	    !memcmp(&((u8 *)buf)[4], &pkcs7_hdr[4], 11)) {
-		msg = pkcs7_parse_message(buf, buflen);
-		if (IS_ERR(msg))
-			return NULL;
-		return msg;
-	}
-
-	/*
-	 * Otherwise, we should add a dummy prefix sequence for pkcs7
-	 * message parser to be able to process.
-	 * NOTE: EDK2 also uses similar hack in WrapPkcs7Data()
-	 * in CryptoPkg/Library/BaseCryptLib/Pk/CryptPkcs7VerifyCommon.c
-	 * TODO:
-	 * The header should be composed in a more refined manner.
-	 */
-	EFI_PRINT("Makeshift prefix added to authentication data\n");
-	ebuflen = sizeof(pkcs7_hdr) + buflen;
-	if (ebuflen <= 0x7f) {
-		EFI_PRINT("Data is too short\n");
-		return NULL;
-	}
-
-	ebuf = malloc(ebuflen);
-	if (!ebuf) {
-		EFI_PRINT("Out of memory\n");
-		return NULL;
-	}
-
-	memcpy(ebuf, pkcs7_hdr, sizeof(pkcs7_hdr));
-	memcpy(ebuf + sizeof(pkcs7_hdr), buf, buflen);
-	len = ebuflen - 4;
-	ebuf[2] = (len >> 8) & 0xff;
-	ebuf[3] = len & 0xff;
-	len = ebuflen - 0x13;
-	ebuf[0x11] = (len >> 8) & 0xff;
-	ebuf[0x12] = len & 0xff;
-
-	msg = pkcs7_parse_message(ebuf, ebuflen);
-
-	if (IS_ERR(msg)) {
-		free(ebuf);
-		return NULL;
-	}
-
-	*tmpbuf = ebuf;
-	return msg;
-}
 
 /**
  * efi_variable_authenticate - authenticate a variable
@@ -215,10 +130,10 @@ static efi_status_t efi_variable_authenticate(u16 *variable,
 		goto err;
 
 	/* ebuf should be kept valid during the authentication */
-	var_sig = efi_variable_parse_signature(auth->auth_info.cert_data,
-					       auth->auth_info.hdr.dwLength
-						   - sizeof(auth->auth_info),
-					       &ebuf);
+	var_sig = efi_parse_pkcs7_header(auth->auth_info.cert_data,
+					 auth->auth_info.hdr.dwLength
+					 - sizeof(auth->auth_info),
+					 &ebuf);
 	if (!var_sig) {
 		EFI_PRINT("Parsing variable's signature failed\n");
 		goto err;
diff --git a/lib/efi_loader/efi_variable_tee.c b/lib/efi_loader/efi_variable_tee.c
index be6f3df..b8808fd 100644
--- a/lib/efi_loader/efi_variable_tee.c
+++ b/lib/efi_loader/efi_variable_tee.c
@@ -36,20 +36,29 @@ static int get_connection(struct mm_connection *conn)
 	static const struct tee_optee_ta_uuid uuid = PTA_STMM_UUID;
 	struct udevice *tee = NULL;
 	struct tee_open_session_arg arg;
-	int rc;
+	int rc = -ENODEV;
 
 	tee = tee_find_device(tee, NULL, NULL, NULL);
 	if (!tee)
-		return -ENODEV;
+		goto out;
 
 	memset(&arg, 0, sizeof(arg));
 	tee_optee_ta_uuid_to_octets(arg.uuid, &uuid);
 	rc = tee_open_session(tee, &arg, 0, NULL);
-	if (!rc) {
-		conn->tee = tee;
-		conn->session = arg.session;
+	if (rc)
+		goto out;
+
+	/* Check the internal OP-TEE result */
+	if (arg.ret != TEE_SUCCESS) {
+		rc = -EIO;
+		goto out;
 	}
 
+	conn->tee = tee;
+	conn->session = arg.session;
+
+	return 0;
+out:
 	return rc;
 }
 
@@ -88,6 +97,7 @@ static efi_status_t optee_mm_communicate(void *comm_buf, ulong dsize)
 
 	if (tee_shm_register(conn.tee, comm_buf, buf_size, 0, &shm)) {
 		log_err("Unable to register shared memory\n");
+		tee_close_session(conn.tee, conn.session);
 		return EFI_UNSUPPORTED;
 	}
 
diff --git a/test/unicode_ut.c b/test/unicode_ut.c
index 33fc8b0..6130ef0 100644
--- a/test/unicode_ut.c
+++ b/test/unicode_ut.c
@@ -603,7 +603,7 @@ static int unicode_test_efi_create_indexed_name(struct unit_test_state *uts)
 	u16 *pos;
 
 	memset(buf, 0xeb, sizeof(buf));
-	pos = efi_create_indexed_name(buf, "Capsule", 0x0af9);
+	pos = efi_create_indexed_name(buf, sizeof(buf), "Capsule", 0x0af9);
 
 	ut_asserteq_mem(expected, buf, sizeof(expected));
 	ut_asserteq(pos - buf, u16_strnlen(buf, SIZE_MAX));
diff --git a/tools/Makefile b/tools/Makefile
index 66d9376..6d7b48f 100644
--- a/tools/Makefile
+++ b/tools/Makefile
@@ -218,6 +218,7 @@ hostprogs-$(CONFIG_MIPS) += mips-relocs
 hostprogs-$(CONFIG_ASN1_COMPILER)	+= asn1_compiler
 HOSTCFLAGS_asn1_compiler.o = -idirafter $(srctree)/include
 
+mkeficapsule-objs	:= mkeficapsule.o $(LIBFDT_OBJS)
 hostprogs-$(CONFIG_EFI_HAVE_CAPSULE_SUPPORT) += mkeficapsule
 
 # We build some files with extra pedantic flags to try to minimize things
diff --git a/tools/mkeficapsule.c b/tools/mkeficapsule.c
index 3f8bc70..270943f 100644
--- a/tools/mkeficapsule.c
+++ b/tools/mkeficapsule.c
@@ -4,16 +4,22 @@
  *		Author: AKASHI Takahiro
  */
 
+#include <errno.h>
 #include <getopt.h>
 #include <malloc.h>
 #include <stdbool.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <unistd.h>
 #include <linux/types.h>
+
+#include <sys/mman.h>
 #include <sys/stat.h>
 #include <sys/types.h>
 
+#include "fdt_host.h"
+
 typedef __u8 u8;
 typedef __u16 u16;
 typedef __u32 u32;
@@ -23,6 +29,9 @@ typedef __s32 s32;
 
 #define aligned_u64 __aligned_u64
 
+#define SIGNATURE_NODENAME	"signature"
+#define OVERLAY_NODENAME	"__overlay__"
+
 #ifndef __packed
 #define __packed __attribute__((packed))
 #endif
@@ -43,6 +52,9 @@ static struct option options[] = {
 	{"raw", required_argument, NULL, 'r'},
 	{"index", required_argument, NULL, 'i'},
 	{"instance", required_argument, NULL, 'I'},
+	{"dtb", required_argument, NULL, 'D'},
+	{"public key", required_argument, NULL, 'K'},
+	{"overlay", no_argument, NULL, 'O'},
 	{"help", no_argument, NULL, 'h'},
 	{NULL, 0, NULL, 0},
 };
@@ -51,14 +63,183 @@ static void print_usage(void)
 {
 	printf("Usage: %s [options] <output file>\n"
 	       "Options:\n"
-	       "\t--fit <fit image>      new FIT image file\n"
-	       "\t--raw <raw image>      new raw image file\n"
-	       "\t--index <index>        update image index\n"
-	       "\t--instance <instance>  update hardware instance\n"
-	       "\t--help                 print a help message\n",
+
+	       "\t--fit <fit image>       new FIT image file\n"
+	       "\t--raw <raw image>       new raw image file\n"
+	       "\t--index <index>         update image index\n"
+	       "\t--instance <instance>   update hardware instance\n"
+	       "\t--public-key <key file> public key esl file\n"
+	       "\t--dtb <dtb file>        dtb file\n"
+	       "\t--overlay               the dtb file is an overlay\n"
+	       "\t--help                  print a help message\n",
 	       tool_name);
 }
 
+static int fdt_add_pub_key_data(void *sptr, void *dptr, size_t key_size,
+				bool overlay)
+{
+	int parent;
+	int ov_node;
+	int frag_node;
+	int ret = 0;
+
+	if (overlay) {
+		/*
+		 * The signature would be stored in the
+		 * first fragment node of the overlay
+		 */
+		frag_node = fdt_first_subnode(dptr, 0);
+		if (frag_node == -FDT_ERR_NOTFOUND) {
+			fprintf(stderr,
+				"Couldn't find the fragment node: %s\n",
+				fdt_strerror(frag_node));
+			goto done;
+		}
+
+		ov_node = fdt_subnode_offset(dptr, frag_node, OVERLAY_NODENAME);
+		if (ov_node == -FDT_ERR_NOTFOUND) {
+			fprintf(stderr,
+				"Couldn't find the __overlay__ node: %s\n",
+				fdt_strerror(ov_node));
+			goto done;
+		}
+	} else {
+		ov_node = 0;
+	}
+
+	parent = fdt_subnode_offset(dptr, ov_node, SIGNATURE_NODENAME);
+	if (parent == -FDT_ERR_NOTFOUND) {
+		parent = fdt_add_subnode(dptr, ov_node, SIGNATURE_NODENAME);
+		if (parent < 0) {
+			ret = parent;
+			if (ret != -FDT_ERR_NOSPACE) {
+				fprintf(stderr,
+					"Couldn't create signature node: %s\n",
+					fdt_strerror(parent));
+			}
+		}
+	}
+	if (ret)
+		goto done;
+
+	/* Write the key to the FDT node */
+	ret = fdt_setprop(dptr, parent, "capsule-key",
+			  sptr, key_size);
+
+done:
+	if (ret)
+		ret = ret == -FDT_ERR_NOSPACE ? -ENOSPC : -EIO;
+
+	return ret;
+}
+
+static int add_public_key(const char *pkey_file, const char *dtb_file,
+			  bool overlay)
+{
+	int ret;
+	int srcfd = 0;
+	int destfd = 0;
+	void *sptr = NULL;
+	void *dptr = NULL;
+	off_t src_size;
+	struct stat pub_key;
+	struct stat dtb;
+
+	/* Find out the size of the public key */
+	srcfd = open(pkey_file, O_RDONLY);
+	if (srcfd == -1) {
+		fprintf(stderr, "%s: Can't open %s: %s\n",
+			__func__, pkey_file, strerror(errno));
+		goto err;
+	}
+
+	ret = fstat(srcfd, &pub_key);
+	if (ret == -1) {
+		fprintf(stderr, "%s: Can't stat %s: %s\n",
+			__func__, pkey_file, strerror(errno));
+		goto err;
+	}
+
+	src_size = pub_key.st_size;
+
+	/* mmap the public key esl file */
+	sptr = mmap(0, src_size, PROT_READ, MAP_SHARED, srcfd, 0);
+	if ((sptr == MAP_FAILED) || (errno != 0)) {
+		fprintf(stderr, "%s: Failed to mmap %s:%s\n",
+			__func__, pkey_file, strerror(errno));
+		goto err;
+	}
+
+	/* Open the dest FDT */
+	destfd = open(dtb_file, O_RDWR);
+	if (destfd == -1) {
+		fprintf(stderr, "%s: Can't open %s: %s\n",
+			__func__, dtb_file, strerror(errno));
+		goto err;
+	}
+
+	ret = fstat(destfd, &dtb);
+	if (ret == -1) {
+		fprintf(stderr, "%s: Can't stat %s: %s\n",
+			__func__, dtb_file, strerror(errno));
+		goto err;
+	}
+
+	dtb.st_size += src_size + 0x30;
+	if (ftruncate(destfd, dtb.st_size)) {
+		fprintf(stderr, "%s: Can't expand %s: %s\n",
+			__func__, dtb_file, strerror(errno));
+		goto err;;
+	}
+
+	errno = 0;
+	/* mmap the dtb file */
+	dptr = mmap(0, dtb.st_size, PROT_READ | PROT_WRITE, MAP_SHARED,
+		    destfd, 0);
+	if ((dptr == MAP_FAILED) || (errno != 0)) {
+		fprintf(stderr, "%s: Failed to mmap %s:%s\n",
+			__func__, dtb_file, strerror(errno));
+		goto err;
+	}
+
+	if (fdt_check_header(dptr)) {
+		fprintf(stderr, "%s: Invalid FDT header\n", __func__);
+		goto err;
+	}
+
+	ret = fdt_open_into(dptr, dptr, dtb.st_size);
+	if (ret) {
+		fprintf(stderr, "%s: Cannot expand FDT: %s\n",
+			__func__, fdt_strerror(ret));
+		goto err;
+	}
+
+	/* Copy the esl file to the expanded FDT */
+	ret = fdt_add_pub_key_data(sptr, dptr, src_size, overlay);
+	if (ret < 0) {
+		fprintf(stderr, "%s: Unable to add public key to the FDT\n",
+			__func__);
+		goto err;
+	}
+
+	return 0;
+
+err:
+	if (sptr)
+		munmap(sptr, src_size);
+
+	if (dptr)
+		munmap(dptr, dtb.st_size);
+
+	if (srcfd >= 0)
+		close(srcfd);
+
+	if (destfd >= 0)
+		close(destfd);
+
+	return -1;
+}
+
 static int create_fwbin(char *path, char *bin, efi_guid_t *guid,
 			unsigned long index, unsigned long instance)
 {
@@ -173,16 +354,22 @@ err_1:
 int main(int argc, char **argv)
 {
 	char *file;
+	char *pkey_file;
+	char *dtb_file;
 	efi_guid_t *guid;
 	unsigned long index, instance;
 	int c, idx;
+	int ret;
+	bool overlay = false;
 
 	file = NULL;
+	pkey_file = NULL;
+	dtb_file = NULL;
 	guid = NULL;
 	index = 0;
 	instance = 0;
 	for (;;) {
-		c = getopt_long(argc, argv, "f:r:i:I:v:h", options, &idx);
+		c = getopt_long(argc, argv, "f:r:i:I:v:D:K:Oh", options, &idx);
 		if (c == -1)
 			break;
 
@@ -209,22 +396,44 @@ int main(int argc, char **argv)
 		case 'I':
 			instance = strtoul(optarg, NULL, 0);
 			break;
+		case 'K':
+			if (pkey_file) {
+				printf("Public Key already specified\n");
+				return -1;
+			}
+			pkey_file = optarg;
+			break;
+		case 'D':
+			if (dtb_file) {
+				printf("DTB file already specified\n");
+				return -1;
+			}
+			dtb_file = optarg;
+			break;
+		case 'O':
+			overlay = true;
+			break;
 		case 'h':
 			print_usage();
 			return 0;
 		}
 	}
 
-	/* need a output file */
-	if (argc != optind + 1) {
+	/* need a fit image file or raw image file */
+	if (!file && !pkey_file && !dtb_file) {
+		printf("%s: %d\n", __func__, __LINE__);
 		print_usage();
 		return -1;
 	}
 
-	/* need a fit image file or raw image file */
-	if (!file) {
-		print_usage();
-		return -1;
+	if (pkey_file && dtb_file) {
+		ret = add_public_key(pkey_file, dtb_file, overlay);
+		if (ret == -1) {
+			printf("Adding public key to the dtb failed\n");
+			return -1;
+		} else {
+			return 0;
+		}
 	}
 
 	if (create_fwbin(argv[optind], file, guid, index, instance)