Merge branch 'next'

26 files changed, 1905 insertions, 211 deletions

diff --git a/lib/Kconfig b/lib/Kconfig
index 7288340614..80ff2443cb 100644
--- a/lib/Kconfig
+++ b/lib/Kconfig
@@ -642,7 +642,7 @@ menu "System tables"
 
 config BLOBLIST_TABLES
 	bool "Put tables in a bloblist"
-	depends on X86
+	depends on X86 && BLOBLIST
 	help
 	  Normally tables are placed at address 0xf0000 and can be up to 64KB
 	  long. With this option, tables are instead placed in the bloblist
diff --git a/lib/Makefile b/lib/Makefile
index edc1c3dd4f..c42d4e1233 100644
--- a/lib/Makefile
+++ b/lib/Makefile
@@ -53,6 +53,7 @@ endif
 obj-$(CONFIG_$(SPL_TPL_)TPM) += tpm-common.o
 ifeq ($(CONFIG_$(SPL_TPL_)TPM),y)
 obj-y += crc8.o
+obj-$(CONFIG_TPM) += tpm_api.o
 obj-$(CONFIG_TPM_V1) += tpm-v1.o
 obj-$(CONFIG_TPM_V2) += tpm-v2.o
 endif
diff --git a/lib/binman.c b/lib/binman.c
index 6040ec8924..530df6a4b4 100644
--- a/lib/binman.c
+++ b/lib/binman.c
@@ -128,8 +128,8 @@ int binman_select_subnode(const char *name)
 	if (!ofnode_valid(node))
 		return log_msg_ret("node", -ENOENT);
 	binman->image = node;
-	log_debug("binman: Selected image subnode '%s'\n",
-		  ofnode_get_name(binman->image));
+	log_info("binman: Selected image subnode '%s'\n",
+		 ofnode_get_name(binman->image));
 
 	return 0;
 }
diff --git a/lib/efi_loader/Kconfig b/lib/efi_loader/Kconfig
index 634d3b1ff4..e44f004f3f 100644
--- a/lib/efi_loader/Kconfig
+++ b/lib/efi_loader/Kconfig
@@ -315,18 +315,13 @@ config EFI_TCG2_PROTOCOL_EVENTLOG_SIZE
 
 config EFI_LOAD_FILE2_INITRD
 	bool "EFI_FILE_LOAD2_PROTOCOL for Linux initial ramdisk"
-	default n
-	help
-	  Expose a EFI_FILE_LOAD2_PROTOCOL that the Linux UEFI stub can
-	  use to load the initial ramdisk. Once this is enabled using
-	  initrd=<ramdisk> will stop working.
-
-config EFI_INITRD_FILESPEC
-	string "initramfs path"
-	default "host 0:1 initrd"
-	depends on EFI_LOAD_FILE2_INITRD
+	default y
 	help
-	  Full path of the initramfs file, e.g. mmc 0:2 initramfs.cpio.gz.
+	  Linux v5.7 and later can make use of this option. If the boot option
+	  selected by the UEFI boot manager specifies an existing file to be used
+	  as initial RAM disk, a Linux specific Load File2 protocol will be
+	  installed and Linux 5.7+ will ignore any initrd=<ramdisk> command line
+	  argument.
 
 config EFI_SECURE_BOOT
 	bool "Enable EFI secure boot support"
@@ -347,4 +342,11 @@ config EFI_SECURE_BOOT
 	  it is signed with a trusted key. To do that, you need to install,
 	  at least, PK, KEK and db.
 
+config EFI_ESRT
+	bool "Enable the UEFI ESRT generation"
+	depends on EFI_CAPSULE_FIRMWARE_MANAGEMENT
+	default y
+	help
+	  Enabling this option creates the ESRT UEFI system table.
+
 endif
diff --git a/lib/efi_loader/Makefile b/lib/efi_loader/Makefile
index 10b42e8847..8bd343e258 100644
--- a/lib/efi_loader/Makefile
+++ b/lib/efi_loader/Makefile
@@ -23,6 +23,7 @@ endif
 obj-$(CONFIG_CMD_BOOTEFI_HELLO) += helloworld_efi.o
 obj-$(CONFIG_CMD_BOOTEFI_BOOTMGR) += efi_bootmgr.o
 obj-y += efi_boottime.o
+obj-y += efi_helper.o
 obj-$(CONFIG_EFI_HAVE_CAPSULE_SUPPORT) += efi_capsule.o
 obj-$(CONFIG_EFI_CAPSULE_FIRMWARE) += efi_firmware.o
 obj-y += efi_console.o
@@ -52,6 +53,7 @@ obj-y += efi_variable.o
 obj-$(CONFIG_EFI_VARIABLES_PRESEED) += efi_var_seed.o
 endif
 obj-y += efi_watchdog.o
+obj-$(CONFIG_EFI_ESRT) += efi_esrt.o
 obj-$(CONFIG_LCD) += efi_gop.o
 obj-$(CONFIG_DM_VIDEO) += efi_gop.o
 obj-$(CONFIG_PARTITIONS) += efi_disk.o
diff --git a/lib/efi_loader/efi_bootmgr.c b/lib/efi_loader/efi_bootmgr.c
index 25f5cebfdb..46c8011344 100644
--- a/lib/efi_loader/efi_bootmgr.c
+++ b/lib/efi_loader/efi_bootmgr.c
@@ -118,11 +118,13 @@ static efi_status_t try_load_entry(u16 n, efi_handle_t *handle,
 		ret = efi_set_variable_int(L"BootCurrent",
 					   &efi_global_variable_guid,
 					   attributes, sizeof(n), &n, false);
-		if (ret != EFI_SUCCESS) {
-			if (EFI_CALL(efi_unload_image(*handle))
-			    != EFI_SUCCESS)
-				log_err("Unloading image failed\n");
-			goto error;
+		if (ret != EFI_SUCCESS)
+			goto unload;
+		/* try to register load file2 for initrd's */
+		if (IS_ENABLED(CONFIG_EFI_LOAD_FILE2_INITRD)) {
+			ret = efi_initrd_register();
+			if (ret != EFI_SUCCESS)
+				goto unload;
 		}
 
 		log_info("Booting: %ls\n", lo.label);
@@ -147,6 +149,13 @@ error:
 	free(load_option);
 
 	return ret;
+
+unload:
+	if (EFI_CALL(efi_unload_image(*handle)) != EFI_SUCCESS)
+		log_err("Unloading image failed\n");
+	free(load_option);
+
+	return ret;
 }
 
 /**
diff --git a/lib/efi_loader/efi_boottime.c b/lib/efi_loader/efi_boottime.c
index 41b8949b04..4777b35fd4 100644
--- a/lib/efi_loader/efi_boottime.c
+++ b/lib/efi_loader/efi_boottime.c
@@ -1406,10 +1406,9 @@ out:
  *
  * Return: status code
  */
-static efi_status_t EFIAPI efi_register_protocol_notify(
-						const efi_guid_t *protocol,
-						struct efi_event *event,
-						void **registration)
+efi_status_t EFIAPI efi_register_protocol_notify(const efi_guid_t *protocol,
+						 struct efi_event *event,
+						 void **registration)
 {
 	struct efi_register_notify_event *item;
 	efi_status_t ret = EFI_SUCCESS;
@@ -1877,7 +1876,6 @@ static
 efi_status_t efi_load_image_from_file(struct efi_device_path *file_path,
 				      void **buffer, efi_uintn_t *size)
 {
-	struct efi_file_info *info = NULL;
 	struct efi_file_handle *f;
 	efi_status_t ret;
 	u64 addr;
@@ -1888,18 +1886,7 @@ efi_status_t efi_load_image_from_file(struct efi_device_path *file_path,
 	if (!f)
 		return EFI_NOT_FOUND;
 
-	/* Get file size */
-	bs = 0;
-	EFI_CALL(ret = f->getinfo(f, (efi_guid_t *)&efi_file_info_guid,
-				  &bs, info));
-	if (ret != EFI_BUFFER_TOO_SMALL) {
-		ret =  EFI_DEVICE_ERROR;
-		goto error;
-	}
-
-	info = malloc(bs);
-	EFI_CALL(ret = f->getinfo(f, (efi_guid_t *)&efi_file_info_guid, &bs,
-				  info));
+	ret = efi_file_size(f, &bs);
 	if (ret != EFI_SUCCESS)
 		goto error;
 
@@ -1909,7 +1896,6 @@ efi_status_t efi_load_image_from_file(struct efi_device_path *file_path,
 	 * allocate a buffer as EFI_BOOT_SERVICES_DATA. The caller has to
 	 * update the reservation according to the image type.
 	 */
-	bs = info->file_size;
 	ret = efi_allocate_pages(EFI_ALLOCATE_ANY_PAGES,
 				 EFI_BOOT_SERVICES_DATA,
 				 efi_size_in_pages(bs), &addr);
@@ -1926,7 +1912,6 @@ efi_status_t efi_load_image_from_file(struct efi_device_path *file_path,
 	*size = bs;
 error:
 	EFI_CALL(f->close(f));
-	free(info);
 	return ret;
 }
 
diff --git a/lib/efi_loader/efi_capsule.c b/lib/efi_loader/efi_capsule.c
index 7ba1ced0a0..9df9c35084 100644
--- a/lib/efi_loader/efi_capsule.c
+++ b/lib/efi_loader/efi_capsule.c
@@ -482,6 +482,14 @@ efi_status_t EFIAPI efi_update_capsule(
 			goto out;
 	}
 out:
+
+	if (IS_ENABLED(CONFIG_EFI_ESRT)) {
+		/* Rebuild the ESRT to reflect any updated FW images. */
+		ret = efi_esrt_populate();
+		if (ret != EFI_SUCCESS)
+			log_warning("EFI Capsule: failed to update ESRT\n");
+	}
+
 	return EFI_EXIT(ret);
 }
 
diff --git a/lib/efi_loader/efi_device_path.c b/lib/efi_loader/efi_device_path.c
index 398dbc699b..4b20859b25 100644
--- a/lib/efi_loader/efi_device_path.c
+++ b/lib/efi_loader/efi_device_path.c
@@ -282,11 +282,31 @@ struct efi_device_path *efi_dp_dup(const struct efi_device_path *dp)
 	return ndp;
 }
 
-struct efi_device_path *efi_dp_append(const struct efi_device_path *dp1,
-				      const struct efi_device_path *dp2)
+/**
+ * efi_dp_append_or_concatenate() - Append or concatenate two device paths.
+ *				    Concatenated device path will be separated
+ *				    by a sub-type 0xff end node
+ *
+ * @dp1:	First device path
+ * @dp2:	Second device path
+ * @concat:	If true the two device paths will be concatenated and separated
+ *		by an end of entrire device path sub-type 0xff end node.
+ *		If true the second device path will be appended to the first and
+ *		terminated by an end node
+ *
+ * Return:
+ * concatenated device path or NULL. Caller must free the returned value
+ */
+static struct
+efi_device_path *efi_dp_append_or_concatenate(const struct efi_device_path *dp1,
+					      const struct efi_device_path *dp2,
+					      bool concat)
 {
 	struct efi_device_path *ret;
+	size_t end_size = sizeof(END);
 
+	if (concat)
+		end_size = 2 * sizeof(END);
 	if (!dp1 && !dp2) {
 		/* return an end node */
 		ret = efi_dp_dup(&END);
@@ -298,18 +318,58 @@ struct efi_device_path *efi_dp_append(const struct efi_device_path *dp1,
 		/* both dp1 and dp2 are non-null */
 		unsigned sz1 = efi_dp_size(dp1);
 		unsigned sz2 = efi_dp_size(dp2);
-		void *p = dp_alloc(sz1 + sz2 + sizeof(END));
+		void *p = dp_alloc(sz1 + sz2 + end_size);
 		if (!p)
 			return NULL;
+		ret = p;
 		memcpy(p, dp1, sz1);
+		p += sz1;
+
+		if (concat) {
+			memcpy(p, &END, sizeof(END));
+			p += sizeof(END);
+		}
+
 		/* the end node of the second device path has to be retained */
-		memcpy(p + sz1, dp2, sz2 + sizeof(END));
-		ret = p;
+		memcpy(p, dp2, sz2);
+		p += sz2;
+		memcpy(p, &END, sizeof(END));
 	}
 
 	return ret;
 }
 
+/**
+ * efi_dp_append() - Append a device to an existing device path.
+ *
+ * @dp1:	First device path
+ * @dp2:	Second device path
+ *
+ * Return:
+ * concatenated device path or NULL. Caller must free the returned value
+ */
+struct efi_device_path *efi_dp_append(const struct efi_device_path *dp1,
+				      const struct efi_device_path *dp2)
+{
+	return efi_dp_append_or_concatenate(dp1, dp2, false);
+}
+
+/**
+ * efi_dp_concat() - Concatenate 2 device paths. The final device path will
+ *                   contain two device paths separated by and end node (0xff).
+ *
+ * @dp1:	First device path
+ * @dp2:	Second device path
+ *
+ * Return:
+ * concatenated device path or NULL. Caller must free the returned value
+ */
+struct efi_device_path *efi_dp_concat(const struct efi_device_path *dp1,
+				      const struct efi_device_path *dp2)
+{
+	return efi_dp_append_or_concatenate(dp1, dp2, true);
+}
+
 struct efi_device_path *efi_dp_append_node(const struct efi_device_path *dp,
 					   const struct efi_device_path *node)
 {
@@ -1183,3 +1243,43 @@ ssize_t efi_dp_check_length(const struct efi_device_path *dp,
 		dp = (const struct efi_device_path *)((const u8 *)dp + len);
 	}
 }
+
+/**
+ * efi_dp_from_lo() - Get the instance of a VenMedia node in a
+ *                    multi-instance device path that matches
+ *                    a specific GUID. This kind of device paths
+ *                    is found in Boot#### options describing an
+ *                    initrd location
+ *
+ * @lo:		EFI_LOAD_OPTION containing a valid device path
+ * @size:	size of the discovered device path
+ * @guid:	guid to search for
+ *
+ * Return:
+ * device path including the VenMedia node or NULL.
+ * Caller must free the returned value.
+ */
+struct
+efi_device_path *efi_dp_from_lo(struct efi_load_option *lo,
+				efi_uintn_t *size, efi_guid_t guid)
+{
+	struct efi_device_path *fp = lo->file_path;
+	struct efi_device_path_vendor *vendor;
+	int lo_len = lo->file_path_length;
+
+	for (; lo_len >=  sizeof(struct efi_device_path);
+	     lo_len -= fp->length, fp = (void *)fp + fp->length) {
+		if (lo_len < 0 || efi_dp_check_length(fp, lo_len) < 0)
+			break;
+		if (fp->type != DEVICE_PATH_TYPE_MEDIA_DEVICE ||
+		    fp->sub_type != DEVICE_PATH_SUB_TYPE_VENDOR_PATH)
+			continue;
+
+		vendor = (struct efi_device_path_vendor *)fp;
+		if (!guidcmp(&vendor->guid, &guid))
+			return efi_dp_dup(fp);
+	}
+	log_debug("VenMedia(%pUl) not found in %ls\n", &guid, lo->label);
+
+	return NULL;
+}
diff --git a/lib/efi_loader/efi_esrt.c b/lib/efi_loader/efi_esrt.c
new file mode 100644
index 0000000000..947bdb5e95
--- /dev/null
+++ b/lib/efi_loader/efi_esrt.c
@@ -0,0 +1,510 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ *  EFI application ESRT tables support
+ *
+ *  Copyright (C) 2021 Arm Ltd.
+ */
+
+#include <common.h>
+#include <efi_loader.h>
+#include <log.h>
+#include <efi_api.h>
+#include <malloc.h>
+
+const efi_guid_t efi_esrt_guid = EFI_SYSTEM_RESOURCE_TABLE_GUID;
+
+static struct efi_system_resource_table *esrt;
+
+#define EFI_ESRT_VERSION 1
+
+/**
+ * efi_esrt_image_info_to_entry() - copy the information present in a fw image
+ * descriptor to a ESRT entry.
+ * The function ensures the ESRT entry matches the image_type_id in @img_info.
+ * In case of a mismatch we leave the entry unchanged.
+ *
+ * @img_info:     the source image info descriptor
+ * @entry:        pointer to the ESRT entry to be filled
+ * @desc_version: the version of the elements in img_info
+ * @image_type:   the image type value to be set in the ESRT entry
+ * @flags:        the capsule flags value to be set in the ESRT entry
+ *
+ * Return:
+ * - EFI_SUCCESS if the entry is correctly updated
+ * - EFI_INVALID_PARAMETER if entry does not match image_type_id in @img_info.
+ */
+static efi_status_t
+efi_esrt_image_info_to_entry(struct efi_firmware_image_descriptor *img_info,
+			     struct efi_system_resource_entry *entry,
+			     u32 desc_version, u32 image_type, u32 flags)
+{
+	if (guidcmp(&entry->fw_class, &img_info->image_type_id)) {
+		EFI_PRINT("ESRT entry %pUL mismatches img_type_id %pUL\n",
+			  &entry->fw_class, &img_info->image_type_id);
+		return EFI_INVALID_PARAMETER;
+	}
+
+	entry->fw_version = img_info->version;
+
+	entry->fw_type = image_type;
+	entry->capsule_flags = flags;
+
+	/*
+	 * The field lowest_supported_image_version is only present
+	 * on image info structure of version 2 or greater.
+	 * See the EFI_FIRMWARE_IMAGE_DESCRIPTOR definition in UEFI.
+	 */
+	if (desc_version >= 2)
+		entry->lowest_supported_fw_version =
+			img_info->lowest_supported_image_version;
+	else
+		entry->lowest_supported_fw_version = 0;
+
+	/*
+	 * The fields last_attempt_version and last_attempt_status
+	 * are only present on image info structure of version 3 or
+	 * greater.
+	 * See the EFI_FIRMWARE_IMAGE_DESCRIPTOR definition in UEFI.
+	 */
+	if (desc_version >= 3) {
+		entry->last_attempt_version =
+			img_info->last_attempt_version;
+
+		entry->last_attempt_status =
+			img_info->last_attempt_status;
+	} else {
+		entry->last_attempt_version = 0;
+		entry->last_attempt_status = LAST_ATTEMPT_STATUS_SUCCESS;
+	}
+
+	return EFI_SUCCESS;
+}
+
+/**
+ * efi_esrt_entries_to_size() - Obtain the bytes used by an ESRT
+ * datastructure with @num_entries.
+ *
+ * @num_entries: the number of entries in the ESRT.
+ *
+ * Return: the number of bytes an ESRT with @num_entries occupies in memory.
+ */
+static
+inline u32 efi_esrt_entries_to_size(u32 num_entries)
+{
+	u32 esrt_size = sizeof(struct efi_system_resource_table) +
+		num_entries * sizeof(struct efi_system_resource_entry);
+
+	return esrt_size;
+}
+
+/**
+ * efi_esrt_allocate_install() - Allocates @num_entries for the ESRT and
+ * performs basic ESRT initialization.
+ *
+ * @num_entries: the number of entries that the ESRT will hold.
+ *
+ * Return:
+ * - pointer to the ESRT if successful.
+ * - NULL otherwise.
+ */
+static
+efi_status_t efi_esrt_allocate_install(u32 num_entries)
+{
+	efi_status_t ret;
+	struct efi_system_resource_table *new_esrt;
+	u32 size = efi_esrt_entries_to_size(num_entries);
+	efi_guid_t esrt_guid = efi_esrt_guid;
+
+	/* Reserve num_pages for ESRT */
+	ret = efi_allocate_pool(EFI_BOOT_SERVICES_DATA, size,
+				(void **)&new_esrt);
+
+	if (ret != EFI_SUCCESS) {
+		EFI_PRINT("ESRT cannot allocate memory for %d entries (%d bytes)\n",
+			  num_entries, efi_esrt_entries_to_size(num_entries));
+
+		return ret;
+	}
+
+	new_esrt->fw_resource_count_max = num_entries;
+	new_esrt->fw_resource_count = 0;
+	new_esrt->fw_resource_version = EFI_ESRT_VERSION;
+
+	/* Install the ESRT in the system configuration table. */
+	ret = efi_install_configuration_table(&esrt_guid, (void *)new_esrt);
+	if (ret != EFI_SUCCESS) {
+		EFI_PRINT("ESRT failed to install the ESRT in the system table\n");
+		return ret;
+	}
+
+	/* If there was a previous ESRT, deallocate its memory now. */
+	if (esrt)
+		ret = EFI_CALL(efi_free_pool(esrt));
+
+	esrt = new_esrt;
+
+	return EFI_SUCCESS;
+}
+
+/**
+ * esrt_find_entry() - Obtain the ESRT entry for the image with GUID
+ * @img_fw_class.
+ *
+ * If the img_fw_class is not yet present in the ESRT, this function
+ * reserves the tail element of the current ESRT as the entry for that fw_class.
+ * The number of elements in the ESRT is updated in that case.
+ *
+ * @img_fw_class: the GUID of the FW image which ESRT entry we want to obtain.
+ *
+ * Return:
+ *  - A pointer to the ESRT entry for the image with GUID img_fw_class,
+ *  - NULL if:
+ *   - there is no more space in the ESRT,
+ *   - ESRT is not initialized,
+ */
+static
+struct efi_system_resource_entry *esrt_find_entry(efi_guid_t *img_fw_class)
+{
+	u32 filled_entries;
+	u32 max_entries;
+	struct efi_system_resource_entry *entry;
+
+	if (!esrt) {
+		EFI_PRINT("ESRT access before initialized\n");
+		return NULL;
+	}
+
+	filled_entries = esrt->fw_resource_count;
+	entry = esrt->entries;
+
+	/* Check if the image with img_fw_class is already in the ESRT. */
+	for (u32 idx = 0; idx < filled_entries; idx++) {
+		if (!guidcmp(&entry[idx].fw_class, img_fw_class)) {
+			EFI_PRINT("ESRT found entry for image %pUl at index %d\n",
+				  img_fw_class, idx);
+			return &entry[idx];
+		}
+	}
+
+	max_entries = esrt->fw_resource_count_max;
+	/*
+	 * Since the image with img_fw_class is not present in the ESRT, check
+	 * if ESRT is full before appending the new entry to it.
+	 */
+	if (filled_entries == max_entries) {
+		EFI_PRINT("ESRT full, this should not happen\n");
+		return NULL;
+	}
+
+	/*
+	 * This is a new entry for a fw image, increment the element
+	 * number in the table and set the fw_class field.
+	 */
+	esrt->fw_resource_count++;
+	entry[filled_entries].fw_class = *img_fw_class;
+	EFI_PRINT("ESRT allocated new entry for image %pUl at index %d\n",
+		  img_fw_class, filled_entries);
+
+	return &entry[filled_entries];
+}
+
+/**
+ * efi_esrt_add_from_fmp() - Populates a sequence of ESRT entries from the FW
+ * images in the FMP.
+ *
+ * @fmp: the FMP instance from which FW images are added to the ESRT
+ *
+ * Return:
+ * - EFI_SUCCESS if all the FW images in the FMP are added to the ESRT
+ * - Error status otherwise
+ */
+static
+efi_status_t efi_esrt_add_from_fmp(struct efi_firmware_management_protocol *fmp)
+{
+	struct efi_system_resource_entry *entry = NULL;
+	size_t info_size = 0;
+	struct efi_firmware_image_descriptor *img_info = NULL;
+	u32 desc_version;
+	u8 desc_count;
+	size_t desc_size;
+	u32 package_version;
+	u16 *package_version_name;
+	efi_status_t ret = EFI_SUCCESS;
+
+	/*
+	 * TODO: set the field image_type depending on the FW image type
+	 * defined in a platform basis.
+	 */
+	u32 image_type = ESRT_FW_TYPE_UNKNOWN;
+
+	/* TODO: set the capsule flags as a function of the FW image type. */
+	u32 flags = 0;
+
+	ret = EFI_CALL(fmp->get_image_info(fmp, &info_size, img_info,
+					   &desc_version, &desc_count,
+					   &desc_size, NULL, NULL));
+
+	if (ret != EFI_BUFFER_TOO_SMALL) {
+		/*
+		 * An input of info_size=0 should always lead
+		 * fmp->get_image_info to return BUFFER_TO_SMALL.
+		 */
+		EFI_PRINT("Erroneous FMP implementation\n");
+		return EFI_INVALID_PARAMETER;
+	}
+
+	ret = EFI_CALL(efi_allocate_pool(EFI_BOOT_SERVICES_DATA, info_size,
+					 (void **)&img_info));
+	if (ret != EFI_SUCCESS) {
+		EFI_PRINT("ESRT failed to allocate memory for image info.\n");
+		return ret;
+	}
+
+	ret = EFI_CALL(fmp->get_image_info(fmp, &info_size, img_info,
+					   &desc_version, &desc_count,
+					   &desc_size, &package_version,
+					   &package_version_name));
+	if (ret != EFI_SUCCESS) {
+		EFI_PRINT("ESRT failed to obtain the FMP image info\n");
+		goto out;
+	}
+
+	/*
+	 * Iterate over all the FW images in the FMP.
+	 */
+	for (u32 desc_idx = 0; desc_idx < desc_count; desc_idx++) {
+		struct efi_firmware_image_descriptor *cur_img_info =
+			(struct efi_firmware_image_descriptor *)
+			((uintptr_t)img_info + desc_idx * desc_size);
+
+		/*
+		 * Obtain the ESRT entry for the FW image with fw_class
+		 * equal to cur_img_info->image_type_id.
+		 */
+		entry = esrt_find_entry(&cur_img_info->image_type_id);
+
+		if (entry) {
+			ret = efi_esrt_image_info_to_entry(cur_img_info, entry,
+							   desc_version,
+							   image_type, flags);
+			if (ret != EFI_SUCCESS)
+				EFI_PRINT("ESRT entry mismatches image_type\n");
+
+		} else {
+			EFI_PRINT("ESRT failed to add entry for %pUl\n",
+				  &cur_img_info->image_type_id);
+			continue;
+		}
+	}
+
+out:
+	EFI_CALL(efi_free_pool(img_info));
+	return EFI_SUCCESS;
+}
+
+/**
+ * efi_esrt_populate() - Populates the ESRT entries from the FMP instances
+ * present in the system.
+ * If an ESRT already exists, the old ESRT is replaced in the system table.
+ * The memory of the old ESRT is deallocated.
+ *
+ * Return:
+ * - EFI_SUCCESS if the ESRT is correctly created
+ * - error code otherwise.
+ */
+efi_status_t efi_esrt_populate(void)
+{
+	efi_handle_t *base_handle = NULL;
+	efi_handle_t *it_handle;
+	size_t no_handles = 0;
+	struct efi_firmware_management_protocol *fmp;
+	efi_status_t ret;
+	u32 num_entries = 0;
+	struct efi_handler *handler;
+
+	/*
+	 * Obtain the number of registered FMP handles.
+	 */
+	ret = EFI_CALL(efi_locate_handle_buffer(BY_PROTOCOL,
+						&efi_guid_firmware_management_protocol,
+						NULL, &no_handles,
+						(efi_handle_t **)&base_handle));
+
+	if (ret != EFI_SUCCESS) {
+		EFI_PRINT("ESRT There are no FMP instances\n");
+
+		ret = efi_esrt_allocate_install(0);
+		if (ret != EFI_SUCCESS) {
+			EFI_PRINT("ESRT failed to create table with 0 entries\n");
+			return ret;
+		}
+		return EFI_SUCCESS;
+	}
+
+	EFI_PRINT("ESRT populate esrt from (%ld) available FMP handles\n",
+		  no_handles);
+
+	/*
+	 * Iterate over all FMPs to determine an upper bound on the number of
+	 * ESRT entries.
+	 */
+	it_handle = base_handle;
+	for (u32 idx = 0; idx < no_handles; idx++, it_handle++) {
+		struct efi_firmware_image_descriptor *img_info = NULL;
+		size_t info_size = 0;
+		u32 desc_version = 0;
+		u8 desc_count = 0;
+		size_t desc_size = 0;
+		u32 package_version;
+		u16 *package_version_name;
+
+		ret = efi_search_protocol(*it_handle,
+					  &efi_guid_firmware_management_protocol,
+					  &handler);
+
+		if (ret != EFI_SUCCESS) {
+			EFI_PRINT("ESRT Unable to find FMP handle (%d)\n",
+				  idx);
+			goto out;
+		}
+		fmp = handler->protocol_interface;
+
+		ret = EFI_CALL(fmp->get_image_info(fmp, &info_size, NULL,
+						   &desc_version, &desc_count,
+						   &desc_size, &package_version,
+						   &package_version_name));
+
+		if (ret != EFI_BUFFER_TOO_SMALL) {
+			/*
+			 * An input of info_size=0 should always lead
+			 * fmp->get_image_info to return BUFFER_TO_SMALL.
+			 */
+			EFI_PRINT("ESRT erroneous FMP implementation\n");
+			ret = EFI_INVALID_PARAMETER;
+			goto out;
+		}
+
+		ret = EFI_CALL(efi_allocate_pool(EFI_BOOT_SERVICES_DATA, info_size,
+						 (void **)&img_info));
+		if (ret != EFI_SUCCESS) {
+			EFI_PRINT("ESRT failed to allocate memory for image info\n");
+			goto out;
+		}
+
+		/*
+		 * Calls to a FMP get_image_info method do not return the
+		 * desc_count value if the return status differs from EFI_SUCCESS.
+		 * We need to repeat the call to get_image_info with a properly
+		 * sized buffer in order to obtain the real number of images
+		 * handled by the FMP.
+		 */
+		ret = EFI_CALL(fmp->get_image_info(fmp, &info_size, img_info,
+						   &desc_version, &desc_count,
+						   &desc_size, &package_version,
+						   &package_version_name));
+
+		if (ret != EFI_SUCCESS) {
+			EFI_PRINT("ESRT failed to obtain image info from FMP\n");
+			EFI_CALL(efi_free_pool(img_info));
+			goto out;
+		}
+
+		num_entries += desc_count;
+
+		EFI_CALL(efi_free_pool(img_info));
+	}
+
+	EFI_PRINT("ESRT create table with %d entries\n", num_entries);
+	/*
+	 * Allocate an ESRT with the sufficient number of entries to accommodate
+	 * all the FMPs in the system.
+	 */
+	ret = efi_esrt_allocate_install(num_entries);
+	if (ret != EFI_SUCCESS) {
+		EFI_PRINT("ESRT failed to initialize table\n");
+		goto out;
+	}
+
+	/*
+	 * Populate the ESRT entries with all existing FMP.
+	 */
+	it_handle = base_handle;
+	for (u32 idx = 0; idx < no_handles; idx++, it_handle++) {
+		ret = EFI_CALL(efi_search_protocol(*it_handle,
+						   &efi_guid_firmware_management_protocol,
+						   &handler));
+
+		if (ret != EFI_SUCCESS) {
+			EFI_PRINT("ESRT unable to find FMP handle (%d)\n",
+				  idx);
+			break;
+		}
+		fmp = handler->protocol_interface;
+
+		ret = efi_esrt_add_from_fmp(fmp);
+		if (ret != EFI_SUCCESS)
+			EFI_PRINT("ESRT failed to add FMP to the table\n");
+	}
+
+out:
+
+	EFI_CALL(efi_free_pool(base_handle));
+
+	return ret;
+}
+
+/**
+ * efi_esrt_new_fmp_notify() - Callback for the EVT_NOTIFY_SIGNAL event raised
+ * when a new FMP protocol instance is registered in the system.
+ */
+static void EFIAPI efi_esrt_new_fmp_notify(struct efi_event *event,
+					   void *context)
+{
+	efi_status_t ret;
+
+	EFI_ENTRY();
+
+	ret = efi_esrt_populate();
+	if (ret != EFI_SUCCESS)
+		EFI_PRINT("ESRT failed to populate ESRT entry\n");
+
+	EFI_EXIT(ret);
+}
+
+/**
+ * efi_esrt_register() - Install the ESRT system table.
+ *
+ * Return: status code
+ */
+efi_status_t efi_esrt_register(void)
+{
+	struct efi_event *ev = NULL;
+	void *registration;
+	efi_status_t ret;
+
+	EFI_PRINT("ESRT creation start\n");
+
+	ret = efi_esrt_populate();
+	if (ret != EFI_SUCCESS) {
+		EFI_PRINT("ESRT failed to initiate the table\n");
+		return ret;
+	}
+
+	ret = EFI_CALL(efi_create_event(EVT_NOTIFY_SIGNAL, TPL_CALLBACK,
+					efi_esrt_new_fmp_notify, NULL, NULL, &ev));
+	if (ret != EFI_SUCCESS) {
+		EFI_PRINT("ESRT failed to create event\n");
+		return ret;
+	}
+
+	ret = EFI_CALL(efi_register_protocol_notify(&efi_guid_firmware_management_protocol,
+						    ev, &registration));
+	if (ret != EFI_SUCCESS) {
+		EFI_PRINT("ESRT failed to register FMP callback\n");
+		return ret;
+	}
+
+	EFI_PRINT("ESRT table created\n");
+
+	return ret;
+}
diff --git a/lib/efi_loader/efi_file.c b/lib/efi_loader/efi_file.c
index 8ece8e71ee..204105e25a 100644
--- a/lib/efi_loader/efi_file.c
+++ b/lib/efi_loader/efi_file.c
@@ -409,6 +409,45 @@ static efi_status_t efi_get_file_size(struct file_handle *fh,
 	return EFI_SUCCESS;
 }
 
+/**
+ * efi_file_size() - Get the size of a file using an EFI file handle
+ *
+ * @fh:		EFI file handle
+ * @size:	buffer to fill in the discovered size
+ *
+ * Return:	size of the file
+ */
+efi_status_t efi_file_size(struct efi_file_handle *fh, efi_uintn_t *size)
+{
+	struct efi_file_info *info = NULL;
+	efi_uintn_t bs = 0;
+	efi_status_t ret;
+
+	*size = 0;
+	ret = EFI_CALL(fh->getinfo(fh, (efi_guid_t *)&efi_file_info_guid, &bs,
+				   info));
+	if (ret != EFI_BUFFER_TOO_SMALL) {
+		ret = EFI_DEVICE_ERROR;
+		goto out;
+	}
+
+	info = malloc(bs);
+	if (!info) {
+		ret = EFI_OUT_OF_RESOURCES;
+		goto out;
+	}
+	ret = EFI_CALL(fh->getinfo(fh, (efi_guid_t *)&efi_file_info_guid, &bs,
+				   info));
+	if (ret != EFI_SUCCESS)
+		goto out;
+
+	*size = info->file_size;
+
+out:
+	free(info);
+	return ret;
+}
+
 static efi_status_t file_read(struct file_handle *fh, u64 *buffer_size,
 		void *buffer)
 {
diff --git a/lib/efi_loader/efi_helper.c b/lib/efi_loader/efi_helper.c
new file mode 100644
index 0000000000..d03a736461
--- /dev/null
+++ b/lib/efi_loader/efi_helper.c
@@ -0,0 +1,98 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright (c) 2020, Linaro Limited
+ */
+
+#define LOG_CATEGORY LOGC_EFI
+#include <common.h>
+#include <env.h>
+#include <malloc.h>
+#include <dm.h>
+#include <fs.h>
+#include <efi_load_initrd.h>
+#include <efi_loader.h>
+#include <efi_variable.h>
+
+/**
+ * efi_create_current_boot_var() - Return Boot#### name were #### is replaced by
+ *			           the value of BootCurrent
+ *
+ * @var_name:		variable name
+ * @var_name_size:	size of var_name
+ *
+ * Return:	Status code
+ */
+static efi_status_t efi_create_current_boot_var(u16 var_name[],
+						size_t var_name_size)
+{
+	efi_uintn_t boot_current_size;
+	efi_status_t ret;
+	u16 boot_current;
+	u16 *pos;
+
+	boot_current_size = sizeof(boot_current);
+	ret = efi_get_variable_int(L"BootCurrent",
+				   &efi_global_variable_guid, NULL,
+				   &boot_current_size, &boot_current, NULL);
+	if (ret != EFI_SUCCESS)
+		goto out;
+
+	pos = efi_create_indexed_name(var_name, var_name_size, "Boot",
+				      boot_current);
+	if (!pos) {
+		ret = EFI_OUT_OF_RESOURCES;
+		goto out;
+	}
+
+out:
+	return ret;
+}
+
+/**
+ * efi_get_dp_from_boot() - Retrieve and return a device path from an EFI
+ *			    Boot### variable.
+ *			    A boot option may contain an array of device paths.
+ *			    We use a VenMedia() with a specific GUID to identify
+ *			    the usage of the array members. This function is
+ *			    used to extract a specific device path
+ *
+ * @guid:	vendor GUID of the VenMedia() device path node identifying the
+ *		device path
+ *
+ * Return:	device path or NULL. Caller must free the returned value
+ */
+struct efi_device_path *efi_get_dp_from_boot(const efi_guid_t guid)
+{
+	struct efi_device_path *file_path = NULL;
+	struct efi_device_path *tmp = NULL;
+	struct efi_load_option lo;
+	void *var_value = NULL;
+	efi_uintn_t size;
+	efi_status_t ret;
+	u16 var_name[16];
+
+	ret = efi_create_current_boot_var(var_name, sizeof(var_name));
+	if (ret != EFI_SUCCESS)
+		return NULL;
+
+	var_value = efi_get_var(var_name, &efi_global_variable_guid, &size);
+	if (!var_value)
+		return NULL;
+
+	ret = efi_deserialize_load_option(&lo, var_value, &size);
+	if (ret != EFI_SUCCESS)
+		goto out;
+
+	tmp = efi_dp_from_lo(&lo, &size, guid);
+	if (!tmp)
+		goto out;
+
+	/* efi_dp_dup will just return NULL if efi_dp_next is NULL */
+	file_path = efi_dp_dup(efi_dp_next(tmp));
+
+out:
+	efi_free_pool(tmp);
+	free(var_value);
+
+	return file_path;
+}
diff --git a/lib/efi_loader/efi_load_initrd.c b/lib/efi_loader/efi_load_initrd.c
index b9ee883905..e2a8063023 100644
--- a/lib/efi_loader/efi_load_initrd.c
+++ b/lib/efi_loader/efi_load_initrd.c
@@ -3,9 +3,11 @@
  * Copyright (c) 2020, Linaro Limited
  */
 
+#define LOG_CATEGORY LOGC_EFI
 #include <common.h>
 #include <efi_loader.h>
 #include <efi_load_initrd.h>
+#include <efi_variable.h>
 #include <fs.h>
 #include <malloc.h>
 #include <mapmem.h>
@@ -23,57 +25,56 @@ static const struct efi_load_file_protocol efi_lf2_protocol = {
  * Device path defined by Linux to identify the handle providing the
  * EFI_LOAD_FILE2_PROTOCOL used for loading the initial ramdisk.
  */
-static const struct efi_initrd_dp dp = {
+static const struct efi_initrd_dp dp_lf2_handle = {
 	.vendor = {
 		{
 		   DEVICE_PATH_TYPE_MEDIA_DEVICE,
 		   DEVICE_PATH_SUB_TYPE_VENDOR_PATH,
-		   sizeof(dp.vendor),
+		   sizeof(dp_lf2_handle.vendor),
 		},
 		EFI_INITRD_MEDIA_GUID,
 	},
 	.end = {
 		DEVICE_PATH_TYPE_END,
 		DEVICE_PATH_SUB_TYPE_END,
-		sizeof(dp.end),
+		sizeof(dp_lf2_handle.end),
 	}
 };
 
+static efi_handle_t efi_initrd_handle;
+
 /**
- * get_file_size() - retrieve the size of initramfs, set efi status on error
+ * get_initrd_fp() - Get initrd device path from a FilePathList device path
  *
- * @dev:			device to read from, e.g. "mmc"
- * @part:			device partition, e.g. "0:1"
- * @file:			name of file
- * @status:			EFI exit code in case of failure
+ * @initrd_fp:	the final initrd filepath
  *
- * Return:			size of file
+ * Return:	status code. Caller must free initrd_fp
  */
-static loff_t get_file_size(const char *dev, const char *part, const char *file,
-			    efi_status_t *status)
+static efi_status_t get_initrd_fp(struct efi_device_path **initrd_fp)
 {
-	loff_t sz = 0;
-	int ret;
-
-	ret = fs_set_blk_dev(dev, part, FS_TYPE_ANY);
-	if (ret) {
-		*status = EFI_NO_MEDIA;
-		goto out;
-	}
+	const efi_guid_t lf2_initrd_guid = EFI_INITRD_MEDIA_GUID;
+	struct efi_device_path *dp = NULL;
 
-	ret = fs_size(file, &sz);
-	if (ret) {
-		sz = 0;
-		*status = EFI_NOT_FOUND;
-		goto out;
-	}
+	/*
+	 * if bootmgr is setup with and initrd, the device path will be
+	 * in the FilePathList[] of our load options in Boot####.
+	 * The first device path of the multi instance device path will
+	 * start with a VenMedia and the initrds will follow.
+	 *
+	 * If the device path is not found return EFI_INVALID_PARAMETER.
+	 * We can then use this specific return value and not install the
+	 * protocol, while allowing the boot to continue
+	 */
+	dp = efi_get_dp_from_boot(lf2_initrd_guid);
+	if (!dp)
+		return EFI_INVALID_PARAMETER;
 
-out:
-	return sz;
+	*initrd_fp = dp;
+	return EFI_SUCCESS;
 }
 
 /**
- * efi_load_file2initrd() - load initial RAM disk
+ * efi_load_file2_initrd() - load initial RAM disk
  *
  * This function implements the LoadFile service of the EFI_LOAD_FILE2_PROTOCOL
  * in order to load an initial RAM disk requested by the Linux kernel stub.
@@ -93,102 +94,125 @@ efi_load_file2_initrd(struct efi_load_file_protocol *this,
 		      struct efi_device_path *file_path, bool boot_policy,
 		      efi_uintn_t *buffer_size, void *buffer)
 {
-	char *filespec;
-	efi_status_t status = EFI_NOT_FOUND;
-	loff_t file_sz = 0, read_sz = 0;
-	char *dev, *part, *file;
-	char *pos;
-	int ret;
+	struct efi_device_path *initrd_fp = NULL;
+	efi_status_t ret = EFI_NOT_FOUND;
+	struct efi_file_handle *f = NULL;
+	efi_uintn_t bs;
 
 	EFI_ENTRY("%p, %p, %d, %p, %p", this, file_path, boot_policy,
 		  buffer_size, buffer);
 
-	filespec = strdup(CONFIG_EFI_INITRD_FILESPEC);
-	if (!filespec)
-		goto out;
-	pos = filespec;
-
 	if (!this || this != &efi_lf2_protocol ||
 	    !buffer_size) {
-		status = EFI_INVALID_PARAMETER;
+		ret = EFI_INVALID_PARAMETER;
 		goto out;
 	}
 
-	if (file_path->type != dp.end.type ||
-	    file_path->sub_type != dp.end.sub_type) {
-		status = EFI_INVALID_PARAMETER;
+	if (file_path->type != dp_lf2_handle.end.type ||
+	    file_path->sub_type != dp_lf2_handle.end.sub_type) {
+		ret = EFI_INVALID_PARAMETER;
 		goto out;
 	}
 
 	if (boot_policy) {
-		status = EFI_UNSUPPORTED;
+		ret = EFI_UNSUPPORTED;
 		goto out;
 	}
 
-	/*
-	 * expect a string with three space separated parts:
-	 *
-	 * * a block device type, e.g. "mmc"
-	 * * a device and partition identifier, e.g. "0:1"
-	 * * a file path on the block device, e.g. "/boot/initrd.cpio.gz"
-	 */
-	dev = strsep(&pos, " ");
-	if (!dev)
+	ret = get_initrd_fp(&initrd_fp);
+	if (ret != EFI_SUCCESS)
 		goto out;
-	part = strsep(&pos, " ");
-	if (!part)
-		goto out;
-	file = strsep(&pos, " ");
-	if (!file)
+
+	/* Open file */
+	f = efi_file_from_path(initrd_fp);
+	if (!f) {
+		log_err("Can't find initrd specified in Boot####\n");
+		ret = EFI_NOT_FOUND;
 		goto out;
+	}
 
-	file_sz = get_file_size(dev, part, file, &status);
-	if (!file_sz)
+	/* Get file size */
+	ret = efi_file_size(f, &bs);
+	if (ret != EFI_SUCCESS)
 		goto out;
 
-	if (!buffer || *buffer_size < file_sz) {
-		status = EFI_BUFFER_TOO_SMALL;
-		*buffer_size = file_sz;
+	if (!buffer || *buffer_size < bs) {
+		ret = EFI_BUFFER_TOO_SMALL;
+		*buffer_size = bs;
 	} else {
-		ret = fs_set_blk_dev(dev, part, FS_TYPE_ANY);
-		if (ret) {
-			status = EFI_NO_MEDIA;
-			goto out;
-		}
-
-		ret = fs_read(file, map_to_sysmem(buffer), 0, *buffer_size,
-			      &read_sz);
-		if (ret || read_sz != file_sz)
-			goto out;
-		*buffer_size = read_sz;
-
-		status = EFI_SUCCESS;
+		ret = EFI_CALL(f->read(f, &bs, (void *)(uintptr_t)buffer));
+		*buffer_size = bs;
+	}
+
+out:
+	efi_free_pool(initrd_fp);
+	if (f)
+		EFI_CALL(f->close(f));
+	return EFI_EXIT(ret);
+}
+
+/**
+ * check_initrd() - Determine if the file defined as an initrd in Boot####
+ *		    load_options device path is present
+ *
+ * Return:	status code
+ */
+static efi_status_t check_initrd(void)
+{
+	struct efi_device_path *initrd_fp = NULL;
+	struct efi_file_handle *f;
+	efi_status_t ret;
+
+	ret = get_initrd_fp(&initrd_fp);
+	if (ret != EFI_SUCCESS)
+		goto out;
+
+	/*
+	 * If the file is not found, but the file path is set, return an error
+	 * and trigger the bootmgr fallback
+	 */
+	f = efi_file_from_path(initrd_fp);
+	if (!f) {
+		log_err("Can't find initrd specified in Boot####\n");
+		ret = EFI_NOT_FOUND;
+		goto out;
 	}
 
+	EFI_CALL(f->close(f));
+
 out:
-	free(filespec);
-	return EFI_EXIT(status);
+	efi_free_pool(initrd_fp);
+	return ret;
 }
 
 /**
  * efi_initrd_register() - create handle for loading initial RAM disk
  *
  * This function creates a new handle and installs a Linux specific vendor
- * device path and an EFI_LOAD_FILE_2_PROTOCOL. Linux uses the device path
+ * device path and an EFI_LOAD_FILE2_PROTOCOL. Linux uses the device path
  * to identify the handle and then calls the LoadFile service of the
- * EFI_LOAD_FILE_2_PROTOCOL to read the initial RAM disk.
+ * EFI_LOAD_FILE2_PROTOCOL to read the initial RAM disk.
  *
  * Return:	status code
  */
 efi_status_t efi_initrd_register(void)
 {
-	efi_handle_t efi_initrd_handle = NULL;
 	efi_status_t ret;
 
+	/*
+	 * Allow the user to continue if Boot#### file path is not set for
+	 * an initrd
+	 */
+	ret = check_initrd();
+	if (ret == EFI_INVALID_PARAMETER)
+		return EFI_SUCCESS;
+	if (ret != EFI_SUCCESS)
+		return ret;
+
 	ret = EFI_CALL(efi_install_multiple_protocol_interfaces
 		       (&efi_initrd_handle,
 			/* initramfs */
-			&efi_guid_device_path, &dp,
+			&efi_guid_device_path, &dp_lf2_handle,
 			/* LOAD_FILE2 */
 			&efi_guid_load_file2_protocol,
 			(void *)&efi_lf2_protocol,
@@ -196,3 +220,17 @@ efi_status_t efi_initrd_register(void)
 
 	return ret;
 }
+
+/**
+ * efi_initrd_deregister() - delete the handle for loading initial RAM disk
+ *
+ * This will delete the handle containing the Linux specific vendor device
+ * path and EFI_LOAD_FILE2_PROTOCOL for loading an initrd
+ *
+ * Return:	status code
+ */
+void efi_initrd_deregister(void)
+{
+	efi_delete_handle(efi_initrd_handle);
+	efi_initrd_handle = NULL;
+}
diff --git a/lib/efi_loader/efi_setup.c b/lib/efi_loader/efi_setup.c
index b1c5125032..3c5cf9a435 100644
--- a/lib/efi_loader/efi_setup.c
+++ b/lib/efi_loader/efi_setup.c
@@ -227,6 +227,12 @@ efi_status_t efi_init_obj_list(void)
 	if (ret != EFI_SUCCESS)
 		goto out;
 
+	if (IS_ENABLED(CONFIG_EFI_ESRT)) {
+		ret = efi_esrt_register();
+		if (ret != EFI_SUCCESS)
+			goto out;
+	}
+
 	if (IS_ENABLED(CONFIG_EFI_TCG2_PROTOCOL)) {
 		ret = efi_tcg2_register();
 		if (ret != EFI_SUCCESS)
diff --git a/lib/efi_loader/efi_tcg2.c b/lib/efi_loader/efi_tcg2.c
index 797d6eb134..09046844c7 100644
--- a/lib/efi_loader/efi_tcg2.c
+++ b/lib/efi_loader/efi_tcg2.c
@@ -13,6 +13,7 @@
 #include <efi_loader.h>
 #include <efi_tcg2.h>
 #include <log.h>
+#include <version.h>
 #include <tpm-v2.h>
 #include <u-boot/sha1.h>
 #include <u-boot/sha256.h>
@@ -958,6 +959,23 @@ out:
 }
 
 /**
+ * tcg2_uninit - remove the final event table and free efi memory on failures
+ */
+void tcg2_uninit(void)
+{
+	efi_status_t ret;
+
+	ret = efi_install_configuration_table(&efi_guid_final_events, NULL);
+	if (ret != EFI_SUCCESS)
+		log_err("Failed to delete final events config table\n");
+
+	efi_free_pool(event_log.buffer);
+	event_log.buffer = NULL;
+	efi_free_pool(event_log.final_buffer);
+	event_log.final_buffer = NULL;
+}
+
+/**
  * create_final_event() - Create the final event and install the config
  *			defined by the TCG EFI spec
  */
@@ -983,10 +1001,6 @@ static efi_status_t create_final_event(void)
 	event_log.final_pos = sizeof(*final_event);
 	ret = efi_install_configuration_table(&efi_guid_final_events,
 					      final_event);
-	if (ret != EFI_SUCCESS)
-		goto out;
-
-	return EFI_SUCCESS;
 out:
 	return ret;
 }
@@ -1041,6 +1055,40 @@ static efi_status_t efi_init_event_log(void)
 	event_log.last_event_size = event_log.pos;
 
 	ret = create_final_event();
+	if (ret != EFI_SUCCESS)
+		goto out;
+
+	return EFI_SUCCESS;
+out:
+	tcg2_uninit();
+	return ret;
+}
+
+/**
+ * efi_append_scrtm_version - Append an S-CRTM EV_S_CRTM_VERSION event on the
+ *			      eventlog and extend the PCRs
+ *
+ * @dev:	TPM device
+ *
+ * @Return:	status code
+ */
+static efi_status_t efi_append_scrtm_version(struct udevice *dev)
+{
+	struct tpml_digest_values digest_list;
+	u8 ver[] = U_BOOT_VERSION_STRING;
+	const int pcr_index = 0;
+	efi_status_t ret;
+
+	ret = tcg2_create_digest(ver, sizeof(ver), &digest_list);
+	if (ret != EFI_SUCCESS)
+		goto out;
+
+	ret = tcg2_pcr_extend(dev, pcr_index, &digest_list);
+	if (ret != EFI_SUCCESS)
+		goto out;
+
+	ret = tcg2_agile_log_append(pcr_index, EV_S_CRTM_VERSION, &digest_list,
+				    sizeof(ver), ver);
 
 out:
 	return ret;
@@ -1055,23 +1103,34 @@ out:
  */
 efi_status_t efi_tcg2_register(void)
 {
-	efi_status_t ret;
+	efi_status_t ret = EFI_SUCCESS;
 	struct udevice *dev;
 
 	ret = platform_get_tpm2_device(&dev);
 	if (ret != EFI_SUCCESS) {
 		log_warning("Unable to find TPMv2 device\n");
-		return EFI_SUCCESS;
+		ret = EFI_SUCCESS;
+		goto out;
 	}
 
 	ret = efi_init_event_log();
 	if (ret != EFI_SUCCESS)
-		return ret;
+		goto fail;
+
+	ret = efi_append_scrtm_version(dev);
+	if (ret != EFI_SUCCESS)
+		goto out;
 
 	ret = efi_add_protocol(efi_root, &efi_guid_tcg2_protocol,
 			       (void *)&efi_tcg2_protocol);
-	if (ret != EFI_SUCCESS)
+	if (ret != EFI_SUCCESS) {
 		log_err("Cannot install EFI_TCG2_PROTOCOL\n");
+		goto fail;
+	}
 
+out:
+	return ret;
+fail:
+	tcg2_uninit();
 	return ret;
 }
diff --git a/lib/efi_loader/efi_var_common.c b/lib/efi_loader/efi_var_common.c
index 1c7459266a..b11ed91a74 100644
--- a/lib/efi_loader/efi_var_common.c
+++ b/lib/efi_loader/efi_var_common.c
@@ -9,6 +9,7 @@
 #include <common.h>
 #include <efi_loader.h>
 #include <efi_variable.h>
+#include <stdlib.h>
 
 enum efi_secure_mode {
 	EFI_MODE_SETUP,
@@ -343,3 +344,35 @@ enum efi_auth_var_type efi_auth_var_get_type(u16 *name, const efi_guid_t *guid)
 	}
 	return EFI_AUTH_VAR_NONE;
 }
+
+/**
+ * efi_get_var() - read value of an EFI variable
+ *
+ * @name:	variable name
+ * @start:	vendor GUID
+ * @size:	size of allocated buffer
+ *
+ * Return:	buffer with variable data or NULL
+ */
+void *efi_get_var(u16 *name, const efi_guid_t *vendor, efi_uintn_t *size)
+{
+	efi_status_t ret;
+	void *buf = NULL;
+
+	*size = 0;
+	ret = efi_get_variable_int(name, vendor, NULL, size, buf, NULL);
+	if (ret == EFI_BUFFER_TOO_SMALL) {
+		buf = malloc(*size);
+		if (!buf)
+			return NULL;
+		ret = efi_get_variable_int(name, vendor, NULL, size, buf, NULL);
+	}
+
+	if (ret != EFI_SUCCESS) {
+		free(buf);
+		*size = 0;
+		return NULL;
+	}
+
+	return buf;
+}
diff --git a/lib/efi_selftest/Makefile b/lib/efi_selftest/Makefile
index 50de581b77..aa71d0995c 100644
--- a/lib/efi_selftest/Makefile
+++ b/lib/efi_selftest/Makefile
@@ -71,6 +71,8 @@ ifeq ($(CONFIG_BLK)$(CONFIG_DOS_PARTITION),yy)
 obj-y += efi_selftest_block_device.o
 endif
 
+obj-$(CONFIG_EFI_ESRT) += efi_selftest_esrt.o
+
 targets += \
 efi_miniapp_file_image_exception.h \
 efi_miniapp_file_image_exit.h \
diff --git a/lib/efi_selftest/efi_selftest.c b/lib/efi_selftest/efi_selftest.c
index b8eed048c2..39ee2edf5d 100644
--- a/lib/efi_selftest/efi_selftest.c
+++ b/lib/efi_selftest/efi_selftest.c
@@ -160,7 +160,7 @@ static bool need_reset(const u16 *testname)
 		if (testname && efi_st_strcmp_16_8(testname, test->name))
 			continue;
 		if (test->phase == EFI_SETUP_BEFORE_BOOTTIME_EXIT ||
-		    test->phase == EFI_SETUP_AFTER_BOOTTIME_EXIT)
+		    test->phase == EFI_SETTING_VIRTUAL_ADDRESS_MAP)
 			return true;
 	}
 	return false;
@@ -327,15 +327,16 @@ efi_status_t EFIAPI efi_selftest(efi_handle_t image_handle,
 	/* Execute mixed tests */
 	efi_st_do_tests(testname, EFI_SETUP_BEFORE_BOOTTIME_EXIT,
 			EFI_ST_SETUP, &failures);
+	efi_st_do_tests(testname, EFI_SETTING_VIRTUAL_ADDRESS_MAP,
+			EFI_ST_SETUP, &failures);
 
 	efi_st_exit_boot_services();
 
 	efi_st_do_tests(testname, EFI_SETUP_BEFORE_BOOTTIME_EXIT,
 			EFI_ST_EXECUTE | EFI_ST_TEARDOWN, &failures);
-
-	/* Execute runtime tests */
-	efi_st_do_tests(testname, EFI_SETUP_AFTER_BOOTTIME_EXIT,
-			EFI_ST_SETUP | EFI_ST_EXECUTE | EFI_ST_TEARDOWN,
+	/* Execute test setting the virtual address map */
+	efi_st_do_tests(testname, EFI_SETTING_VIRTUAL_ADDRESS_MAP,
+			EFI_ST_EXECUTE | EFI_ST_TEARDOWN,
 			&failures);
 
 	/* Give feedback */
diff --git a/lib/efi_selftest/efi_selftest_esrt.c b/lib/efi_selftest/efi_selftest_esrt.c
new file mode 100644
index 0000000000..99251f22a5
--- /dev/null
+++ b/lib/efi_selftest/efi_selftest_esrt.c
@@ -0,0 +1,291 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ *  Test ESRT tables support
+ *
+ *  Copyright (C) 2021 Arm Ltd.
+ */
+#include <common.h>
+#include <efi_loader.h>
+#include <efi_selftest.h>
+
+// This value must not exceed 255.
+// An FMP cannot contain more than 255 FW images.
+#define TEST_ESRT_NUM_ENTRIES 255
+
+static
+struct efi_firmware_image_descriptor static_img_info[TEST_ESRT_NUM_ENTRIES];
+
+static const struct efi_system_table *local_systable;
+
+static efi_handle_t fmp_handle;
+
+static const efi_guid_t efi_fmp_guid =
+		EFI_FIRMWARE_MANAGEMENT_PROTOCOL_GUID;
+
+static void efi_test_esrt_init_info(void)
+{
+	for (int idx = 0; idx < TEST_ESRT_NUM_ENTRIES; idx++) {
+		static_img_info[idx].image_index = idx;
+
+		// Note: the 16 byte value present in
+		// static_img_info[idx].image_type_id is not strictly a GUID.
+		// The value is used for the sake of code testing.
+		static_img_info[idx].image_type_id.b[0] = idx;
+
+		static_img_info[idx].image_id = 0;
+		static_img_info[idx].image_id_name = NULL;
+		static_img_info[idx].version = 0;
+		static_img_info[idx].version_name = NULL;
+		static_img_info[idx].size = 0;
+		static_img_info[idx].lowest_supported_image_version = 1;
+		static_img_info[idx].last_attempt_version = 2;
+		static_img_info[idx].last_attempt_status = 3;
+		static_img_info[idx].hardware_instance = 1;
+	}
+}
+
+static efi_status_t
+EFIAPI efi_test_fmp_get_image_info(struct efi_firmware_management_protocol *this,
+				   efi_uintn_t *image_info_size,
+				   struct efi_firmware_image_descriptor *image_info,
+				   u32 *descriptor_version,
+				   u8 *descriptor_count,
+				   efi_uintn_t *descriptor_size,
+				   u32 *package_version,
+				   u16 **package_version_name)
+{
+	efi_status_t ret = EFI_SUCCESS;
+
+	if (!image_info_size)
+		return EFI_INVALID_PARAMETER;
+
+	if (descriptor_version)
+		*descriptor_version = EFI_FIRMWARE_IMAGE_DESCRIPTOR_VERSION;
+	if (descriptor_count)
+		*descriptor_count = TEST_ESRT_NUM_ENTRIES;
+	if (descriptor_size)
+		*descriptor_size = sizeof(*image_info);
+	if (package_version)
+		*package_version = 0xffffffff;
+	if (package_version_name)
+		*package_version_name = NULL;
+
+	if (*image_info_size < sizeof(*image_info)) {
+		*image_info_size = *descriptor_size * *descriptor_count;
+		return EFI_BUFFER_TOO_SMALL;
+	}
+
+	for (int idx = 0; idx < TEST_ESRT_NUM_ENTRIES; idx++)
+		image_info[idx] = static_img_info[idx];
+
+	return ret;
+}
+
+static struct efi_firmware_management_protocol efi_test_fmp = {
+	.get_image_info = efi_test_fmp_get_image_info,
+	.get_image = NULL,
+	.set_image = NULL,
+	.check_image = NULL,
+	.get_package_info = NULL,
+	.set_package_info = NULL,
+};
+
+static void *lib_test_get_esrt(void)
+{
+	for (int idx = 0; idx < local_systable->nr_tables; idx++)
+		if (!guidcmp(&efi_esrt_guid, &local_systable->tables[idx].guid))
+			return local_systable->tables[idx].table;
+
+	return NULL;
+}
+
+/**
+ * lib_test_check_uuid_entry: Find an ESRT entry for which the fw_calss field matches
+ * the image_type_id in the @img_info.
+ * Ensure that all of the field in the ESRT entry have the same value as the corresponding
+ * fields in the @img_info.
+ *
+ * @esrt: pointer to the ESRT
+ * @img_info: an image_info_descriptor output by the FMP get_image_info
+ *
+ * @return: true if matching ESRT entry is found and if all the ESRT entry fields match the
+ * corresponding @img_info fields.
+ */
+static bool lib_test_check_uuid_entry(struct efi_system_resource_table *esrt,
+				      struct efi_firmware_image_descriptor
+				      *img_info)
+{
+	const u32 filled_entries = esrt->fw_resource_count;
+	struct efi_system_resource_entry *entry = esrt->entries;
+
+	for (u32 idx = 0; idx < filled_entries; idx++) {
+		if (!guidcmp(&entry[idx].fw_class, &img_info->image_type_id)) {
+			if (entry[idx].fw_version != img_info->version) {
+				efi_st_error("ESRT field mismatch for entry with fw_class=%pUl\n",
+					     &img_info->image_type_id);
+				return false;
+			}
+
+			if (entry[idx].lowest_supported_fw_version !=
+				img_info->lowest_supported_image_version) {
+				efi_st_error("ESRT field mismatch for entry with fw_class=%pUl\n",
+					     &img_info->image_type_id);
+				return false;
+			}
+
+			if (entry[idx].last_attempt_version !=
+				img_info->last_attempt_version) {
+				efi_st_error("ESRT field mismatch for entry with fw_class=%pUl\n",
+					     &img_info->image_type_id);
+				return false;
+			}
+
+			if (entry[idx].last_attempt_status !=
+				img_info->last_attempt_status) {
+				efi_st_error("ESRT field mismatch for entry with fw_class=%pUl\n",
+					     &img_info->image_type_id);
+				return false;
+			}
+
+			/*
+			 * The entry with fw_class = img_uuid matches with the
+			 * remainder fmp input.
+			 */
+			return true;
+		}
+	}
+
+	/* There exists no entry with fw_class equal to img_uuid in the ESRT. */
+	efi_st_error("ESRT no entry with fw_class= %pUl\n", &img_info->image_type_id);
+
+	return false;
+}
+
+/*
+ * Setup unit test.
+ *
+ * Initialize the test FMP datastructure.
+ *
+ * @handle:	handle of the loaded image
+ * @systable:	system table
+ * @return:	EFI_ST_SUCCESS for success
+ */
+static int setup(const efi_handle_t handle,
+		 const struct efi_system_table *systable)
+{
+	local_systable = systable;
+
+	efi_test_esrt_init_info();
+
+	return EFI_ST_SUCCESS;
+}
+
+/*
+ * Tear down unit test.
+ *
+ * Uninstall the test FMP.
+ *
+ * @return:	EFI_ST_SUCCESS for success
+ */
+static int teardown(void)
+{
+	efi_status_t ret = EFI_SUCCESS;
+	struct efi_boot_services *bt;
+
+	bt = local_systable->boottime;
+
+	if (!bt) {
+		efi_st_error("Cannot find boottime services structure\n");
+		return EFI_ST_FAILURE;
+	}
+
+	ret = bt->uninstall_multiple_protocol_interfaces
+		(fmp_handle, &efi_fmp_guid,
+		 &efi_test_fmp, NULL);
+
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Failed to uninstall FMP\n");
+		return EFI_ST_FAILURE;
+	}
+
+	return EFI_ST_SUCCESS;
+}
+
+/*
+ * Perform the test
+ *
+ * The test consists of the following steps:
+ *
+ * 1) Obtain the ESRT
+ * 2) Record the number of ESRT entries prior to test start
+ * 3) Install the test FMP
+ * 4) Re-obtain the ESRT (the ESRT pointer may have changed with the FMP install)
+ * 5) verify that the ESRT entries have increased by the number of entries in the
+ *     test FMP.
+ * 6) Traverse all the elements used as the test FMP input and verify that each
+ *     has a corresponding ESRT entry and that the fields are correctly set.
+ *
+ * The failure of any of the above steps results in a test failure.
+ *
+ */
+static int execute(void)
+{
+	struct efi_system_resource_table *esrt;
+	efi_status_t ret = EFI_SUCCESS;
+	u32 base_entry_count;
+	u32 entry_delta;
+	struct efi_boot_services *bt;
+
+	bt = local_systable->boottime;
+
+	if (!bt) {
+		efi_st_error("Cannot find boottime services structure\n");
+		return EFI_ST_FAILURE;
+	}
+
+	esrt = lib_test_get_esrt();
+	if (!esrt) {
+		efi_st_error("ESRT table not present\n");
+		return EFI_ST_FAILURE;
+	}
+	base_entry_count = esrt->fw_resource_count;
+
+	ret = bt->install_multiple_protocol_interfaces(&fmp_handle,
+						       &efi_fmp_guid,
+						       &efi_test_fmp,
+						       NULL);
+
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Failed to install FMP\n");
+		return EFI_ST_FAILURE;
+	}
+
+	esrt = lib_test_get_esrt();
+	if (!esrt) {
+		efi_st_error("ESRT table not present\n");
+		return EFI_ST_FAILURE;
+	}
+
+	entry_delta = esrt->fw_resource_count - base_entry_count;
+	if (entry_delta != TEST_ESRT_NUM_ENTRIES) {
+		efi_st_error("ESRT mismatch in new entry count (%d), expected (%d).\n",
+			     entry_delta, TEST_ESRT_NUM_ENTRIES);
+		return EFI_ST_FAILURE;
+	}
+
+	for (u32 idx = 0; idx < TEST_ESRT_NUM_ENTRIES; idx++)
+		if (!lib_test_check_uuid_entry(esrt, &static_img_info[idx])) {
+			efi_st_error("ESRT entry mismatch\n");
+			return EFI_ST_FAILURE;
+		}
+
+	return EFI_ST_SUCCESS;
+}
+
+EFI_UNIT_TEST(esrt) = {
+	.name = "esrt",
+	.phase = EFI_EXECUTE_BEFORE_BOOTTIME_EXIT,
+	.setup = setup,
+	.execute = execute,
+	.teardown = teardown,
+};
diff --git a/lib/efi_selftest/efi_selftest_set_virtual_address_map.c b/lib/efi_selftest/efi_selftest_set_virtual_address_map.c
index b097a81136..8e2e8ba172 100644
--- a/lib/efi_selftest/efi_selftest_set_virtual_address_map.c
+++ b/lib/efi_selftest/efi_selftest_set_virtual_address_map.c
@@ -201,7 +201,7 @@ static int execute(void)
 
 EFI_UNIT_TEST(virtaddrmap) = {
 	.name = "virtual address map",
-	.phase = EFI_SETUP_BEFORE_BOOTTIME_EXIT,
+	.phase = EFI_SETTING_VIRTUAL_ADDRESS_MAP,
 	.setup = setup,
 	.execute = execute,
 };
diff --git a/lib/smbios-parser.c b/lib/smbios-parser.c
index b89f988ef9..34203f952c 100644
--- a/lib/smbios-parser.c
+++ b/lib/smbios-parser.c
@@ -3,6 +3,8 @@
  * Copyright (C) 2020, Bachmann electronic GmbH
  */
 
+#define LOG_CATEGORY	LOGC_BOOT
+
 #include <common.h>
 #include <smbios.h>
 
@@ -94,3 +96,39 @@ const char *smbios_string(const struct smbios_header *header, int index)
 
 	return string_from_smbios_table(header, index);
 }
+
+int smbios_update_version_full(void *smbios_tab, const char *version)
+{
+	const struct smbios_header *hdr;
+	struct smbios_type0 *bios;
+	uint old_len, len;
+	char *ptr;
+
+	log_info("Updating SMBIOS table at %p\n", smbios_tab);
+	hdr = smbios_header(smbios_tab, SMBIOS_BIOS_INFORMATION);
+	if (!hdr)
+		return log_msg_ret("tab", -ENOENT);
+	bios = (struct smbios_type0 *)hdr;
+	ptr = (char *)smbios_string(hdr, bios->bios_ver);
+	if (!ptr)
+		return log_msg_ret("str", -ENOMEDIUM);
+
+	/*
+	 * This string is supposed to have at least enough bytes and is
+	 * padded with spaces. Update it, taking care not to move the
+	 * \0 terminator, so that other strings in the string table
+	 * are not disturbed. See smbios_add_string()
+	 */
+	old_len = strnlen(ptr, SMBIOS_STR_MAX);
+	len = strnlen(version, SMBIOS_STR_MAX);
+	if (len > old_len)
+		return log_ret(-ENOSPC);
+
+	log_debug("Replacing SMBIOS type 0 version string '%s'\n", ptr);
+	memcpy(ptr, version, len);
+#ifdef LOG_DEBUG
+	print_buffer((ulong)ptr, ptr, 1, old_len + 1, 0);
+#endif
+
+	return 0;
+}
diff --git a/lib/smbios.c b/lib/smbios.c
index 7d463c84a9..9eb226ec9f 100644
--- a/lib/smbios.c
+++ b/lib/smbios.c
@@ -20,10 +20,6 @@
 
 DECLARE_GLOBAL_DATA_PTR;
 
-enum {
-	SMBIOS_STR_MAX	= 64,	/* Maximum length allowed for a string */
-};
-
 /**
  * struct smbios_ctx - context for writing SMBIOS tables
  *
diff --git a/lib/tpm-common.c b/lib/tpm-common.c
index e4af87f76a..4277846fdd 100644
--- a/lib/tpm-common.c
+++ b/lib/tpm-common.c
@@ -166,6 +166,7 @@ u32 tpm_sendrecv_command(struct udevice *dev, const void *command,
 	u8 response_buffer[COMMAND_BUFFER_SIZE];
 	size_t response_length;
 	int i;
+	uint size;
 
 	if (response) {
 		response_length = *size_ptr;
@@ -174,8 +175,13 @@ u32 tpm_sendrecv_command(struct udevice *dev, const void *command,
 		response_length = sizeof(response_buffer);
 	}
 
-	err = tpm_xfer(dev, command, tpm_command_size(command),
-		       response, &response_length);
+	size = tpm_command_size(command);
+	log_debug("TPM request [size:%d]: ", size);
+	for (i = 0; i < size; i++)
+		log_debug("%02x ", ((u8 *)command)[i]);
+	log_debug("\n");
+
+	err = tpm_xfer(dev, command, size, response, &response_length);
 
 	if (err < 0)
 		return err;
diff --git a/lib/tpm-v1.c b/lib/tpm-v1.c
index a846fe00dd..8dc144080c 100644
--- a/lib/tpm-v1.c
+++ b/lib/tpm-v1.c
@@ -32,7 +32,7 @@ static struct session_data oiap_session = {0, };
 
 #endif /* CONFIG_TPM_AUTH_SESSIONS */
 
-u32 tpm_startup(struct udevice *dev, enum tpm_startup_type mode)
+u32 tpm1_startup(struct udevice *dev, enum tpm_startup_type mode)
 {
 	const u8 command[12] = {
 		0x0, 0xc1, 0x0, 0x0, 0x0, 0xc, 0x0, 0x0, 0x0, 0x99, 0x0, 0x0,
@@ -48,12 +48,12 @@ u32 tpm_startup(struct udevice *dev, enum tpm_startup_type mode)
 	return tpm_sendrecv_command(dev, buf, NULL, NULL);
 }
 
-u32 tpm_resume(struct udevice *dev)
+u32 tpm1_resume(struct udevice *dev)
 {
-	return tpm_startup(dev, TPM_ST_STATE);
+	return tpm1_startup(dev, TPM_ST_STATE);
 }
 
-u32 tpm_self_test_full(struct udevice *dev)
+u32 tpm1_self_test_full(struct udevice *dev)
 {
 	const u8 command[10] = {
 		0x0, 0xc1, 0x0, 0x0, 0x0, 0xa, 0x0, 0x0, 0x0, 0x50,
@@ -61,7 +61,7 @@ u32 tpm_self_test_full(struct udevice *dev)
 	return tpm_sendrecv_command(dev, command, NULL, NULL);
 }
 
-u32 tpm_continue_self_test(struct udevice *dev)
+u32 tpm1_continue_self_test(struct udevice *dev)
 {
 	const u8 command[10] = {
 		0x0, 0xc1, 0x0, 0x0, 0x0, 0xa, 0x0, 0x0, 0x0, 0x53,
@@ -69,35 +69,33 @@ u32 tpm_continue_self_test(struct udevice *dev)
 	return tpm_sendrecv_command(dev, command, NULL, NULL);
 }
 
-u32 tpm_clear_and_reenable(struct udevice *dev)
+u32 tpm1_clear_and_reenable(struct udevice *dev)
 {
 	u32 ret;
 
 	log_info("TPM: Clear and re-enable\n");
-	ret = tpm_force_clear(dev);
+	ret = tpm1_force_clear(dev);
 	if (ret != TPM_SUCCESS) {
 		log_err("Can't initiate a force clear\n");
 		return ret;
 	}
 
-	if (tpm_get_version(dev) == TPM_V1) {
-		ret = tpm_physical_enable(dev);
-		if (ret != TPM_SUCCESS) {
-			log_err("TPM: Can't set enabled state\n");
-			return ret;
-		}
+	ret = tpm1_physical_enable(dev);
+	if (ret != TPM_SUCCESS) {
+		log_err("TPM: Can't set enabled state\n");
+		return ret;
+	}
 
-		ret = tpm_physical_set_deactivated(dev, 0);
-		if (ret != TPM_SUCCESS) {
-			log_err("TPM: Can't set deactivated state\n");
-			return ret;
-		}
+	ret = tpm1_physical_set_deactivated(dev, 0);
+	if (ret != TPM_SUCCESS) {
+		log_err("TPM: Can't set deactivated state\n");
+		return ret;
 	}
 
 	return TPM_SUCCESS;
 }
 
-u32 tpm_nv_define_space(struct udevice *dev, u32 index, u32 perm, u32 size)
+u32 tpm1_nv_define_space(struct udevice *dev, u32 index, u32 perm, u32 size)
 {
 	const u8 command[101] = {
 		0x0, 0xc1,		/* TPM_TAG */
@@ -140,12 +138,12 @@ u32 tpm_nv_define_space(struct udevice *dev, u32 index, u32 perm, u32 size)
 	return tpm_sendrecv_command(dev, buf, NULL, NULL);
 }
 
-u32 tpm_nv_set_locked(struct udevice *dev)
+u32 tpm1_nv_set_locked(struct udevice *dev)
 {
-	return tpm_nv_define_space(dev, TPM_NV_INDEX_LOCK, 0, 0);
+	return tpm1_nv_define_space(dev, TPM_NV_INDEX_LOCK, 0, 0);
 }
 
-u32 tpm_nv_read_value(struct udevice *dev, u32 index, void *data, u32 count)
+u32 tpm1_nv_read_value(struct udevice *dev, u32 index, void *data, u32 count)
 {
 	const u8 command[22] = {
 		0x0, 0xc1, 0x0, 0x0, 0x0, 0x16, 0x0, 0x0, 0x0, 0xcf,
@@ -179,8 +177,8 @@ u32 tpm_nv_read_value(struct udevice *dev, u32 index, void *data, u32 count)
 	return 0;
 }
 
-u32 tpm_nv_write_value(struct udevice *dev, u32 index, const void *data,
-		       u32 length)
+u32 tpm1_nv_write_value(struct udevice *dev, u32 index, const void *data,
+			u32 length)
 {
 	const u8 command[256] = {
 		0x0, 0xc1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xcd,
@@ -210,13 +208,8 @@ u32 tpm_nv_write_value(struct udevice *dev, u32 index, const void *data,
 	return 0;
 }
 
-uint32_t tpm_set_global_lock(struct udevice *dev)
-{
-	return tpm_nv_write_value(dev, TPM_NV_INDEX_0, NULL, 0);
-}
-
-u32 tpm_extend(struct udevice *dev, u32 index, const void *in_digest,
-	       void *out_digest)
+u32 tpm1_extend(struct udevice *dev, u32 index, const void *in_digest,
+		void *out_digest)
 {
 	const u8 command[34] = {
 		0x0, 0xc1, 0x0, 0x0, 0x0, 0x22, 0x0, 0x0, 0x0, 0x14,
@@ -247,7 +240,7 @@ u32 tpm_extend(struct udevice *dev, u32 index, const void *in_digest,
 	return 0;
 }
 
-u32 tpm_pcr_read(struct udevice *dev, u32 index, void *data, size_t count)
+u32 tpm1_pcr_read(struct udevice *dev, u32 index, void *data, size_t count)
 {
 	const u8 command[14] = {
 		0x0, 0xc1, 0x0, 0x0, 0x0, 0xe, 0x0, 0x0, 0x0, 0x15,
@@ -275,7 +268,7 @@ u32 tpm_pcr_read(struct udevice *dev, u32 index, void *data, size_t count)
 	return 0;
 }
 
-u32 tpm_tsc_physical_presence(struct udevice *dev, u16 presence)
+u32 tpm1_tsc_physical_presence(struct udevice *dev, u16 presence)
 {
 	const u8 command[12] = {
 		0x0, 0xc1, 0x0, 0x0, 0x0, 0xc, 0x40, 0x0, 0x0, 0xa, 0x0, 0x0,
@@ -291,7 +284,7 @@ u32 tpm_tsc_physical_presence(struct udevice *dev, u16 presence)
 	return tpm_sendrecv_command(dev, buf, NULL, NULL);
 }
 
-u32 tpm_finalise_physical_presence(struct udevice *dev)
+u32 tpm1_finalise_physical_presence(struct udevice *dev)
 {
 	const u8 command[12] = {
 		0x0, 0xc1, 0x0, 0x0, 0x0, 0xc, 0x40, 0x0, 0x0, 0xa, 0x2, 0xa0,
@@ -300,7 +293,7 @@ u32 tpm_finalise_physical_presence(struct udevice *dev)
 	return tpm_sendrecv_command(dev, command, NULL, NULL);
 }
 
-u32 tpm_read_pubek(struct udevice *dev, void *data, size_t count)
+u32 tpm1_read_pubek(struct udevice *dev, void *data, size_t count)
 {
 	const u8 command[30] = {
 		0x0, 0xc1, 0x0, 0x0, 0x0, 0x1e, 0x0, 0x0, 0x0, 0x7c,
@@ -331,7 +324,7 @@ u32 tpm_read_pubek(struct udevice *dev, void *data, size_t count)
 	return 0;
 }
 
-u32 tpm_force_clear(struct udevice *dev)
+u32 tpm1_force_clear(struct udevice *dev)
 {
 	const u8 command[10] = {
 		0x0, 0xc1, 0x0, 0x0, 0x0, 0xa, 0x0, 0x0, 0x0, 0x5d,
@@ -340,7 +333,7 @@ u32 tpm_force_clear(struct udevice *dev)
 	return tpm_sendrecv_command(dev, command, NULL, NULL);
 }
 
-u32 tpm_physical_enable(struct udevice *dev)
+u32 tpm1_physical_enable(struct udevice *dev)
 {
 	const u8 command[10] = {
 		0x0, 0xc1, 0x0, 0x0, 0x0, 0xa, 0x0, 0x0, 0x0, 0x6f,
@@ -349,7 +342,7 @@ u32 tpm_physical_enable(struct udevice *dev)
 	return tpm_sendrecv_command(dev, command, NULL, NULL);
 }
 
-u32 tpm_physical_disable(struct udevice *dev)
+u32 tpm1_physical_disable(struct udevice *dev)
 {
 	const u8 command[10] = {
 		0x0, 0xc1, 0x0, 0x0, 0x0, 0xa, 0x0, 0x0, 0x0, 0x70,
@@ -358,7 +351,7 @@ u32 tpm_physical_disable(struct udevice *dev)
 	return tpm_sendrecv_command(dev, command, NULL, NULL);
 }
 
-u32 tpm_physical_set_deactivated(struct udevice *dev, u8 state)
+u32 tpm1_physical_set_deactivated(struct udevice *dev, u8 state)
 {
 	const u8 command[11] = {
 		0x0, 0xc1, 0x0, 0x0, 0x0, 0xb, 0x0, 0x0, 0x0, 0x72,
@@ -374,8 +367,8 @@ u32 tpm_physical_set_deactivated(struct udevice *dev, u8 state)
 	return tpm_sendrecv_command(dev, buf, NULL, NULL);
 }
 
-u32 tpm_get_capability(struct udevice *dev, u32 cap_area, u32 sub_cap,
-		       void *cap, size_t count)
+u32 tpm1_get_capability(struct udevice *dev, u32 cap_area, u32 sub_cap,
+			void *cap, size_t count)
 {
 	const u8 command[22] = {
 		0x0, 0xc1,		/* TPM_TAG */
@@ -414,8 +407,8 @@ u32 tpm_get_capability(struct udevice *dev, u32 cap_area, u32 sub_cap,
 	return 0;
 }
 
-u32 tpm_get_permanent_flags(struct udevice *dev,
-			    struct tpm_permanent_flags *pflags)
+u32 tpm1_get_permanent_flags(struct udevice *dev,
+			     struct tpm_permanent_flags *pflags)
 {
 	const u8 command[22] = {
 		0x0, 0xc1,		/* TPM_TAG */
@@ -453,7 +446,7 @@ u32 tpm_get_permanent_flags(struct udevice *dev,
 	return 0;
 }
 
-u32 tpm_get_permissions(struct udevice *dev, u32 index, u32 *perm)
+u32 tpm1_get_permissions(struct udevice *dev, u32 index, u32 *perm)
 {
 	const u8 command[22] = {
 		0x0, 0xc1,		/* TPM_TAG */
@@ -482,7 +475,7 @@ u32 tpm_get_permissions(struct udevice *dev, u32 index, u32 *perm)
 }
 
 #ifdef CONFIG_TPM_FLUSH_RESOURCES
-u32 tpm_flush_specific(struct udevice *dev, u32 key_handle, u32 resource_type)
+u32 tpm1_flush_specific(struct udevice *dev, u32 key_handle, u32 resource_type)
 {
 	const u8 command[18] = {
 		0x00, 0xc1,             /* TPM_TAG */
@@ -641,7 +634,7 @@ static u32 verify_response_auth(u32 command_code, const void *response,
 	return TPM_SUCCESS;
 }
 
-u32 tpm_terminate_auth_session(struct udevice *dev, u32 auth_handle)
+u32 tpm1_terminate_auth_session(struct udevice *dev, u32 auth_handle)
 {
 	const u8 command[18] = {
 		0x00, 0xc1,		/* TPM_TAG */
@@ -663,16 +656,16 @@ u32 tpm_terminate_auth_session(struct udevice *dev, u32 auth_handle)
 	return tpm_sendrecv_command(dev, request, NULL, NULL);
 }
 
-u32 tpm_end_oiap(struct udevice *dev)
+u32 tpm1_end_oiap(struct udevice *dev)
 {
 	u32 err = TPM_SUCCESS;
 
 	if (oiap_session.valid)
-		err = tpm_terminate_auth_session(dev, oiap_session.handle);
+		err = tpm1_terminate_auth_session(dev, oiap_session.handle);
 	return err;
 }
 
-u32 tpm_oiap(struct udevice *dev, u32 *auth_handle)
+u32 tpm1_oiap(struct udevice *dev, u32 *auth_handle)
 {
 	const u8 command[10] = {
 		0x00, 0xc1,		/* TPM_TAG */
@@ -686,7 +679,7 @@ u32 tpm_oiap(struct udevice *dev, u32 *auth_handle)
 	u32 err;
 
 	if (oiap_session.valid)
-		tpm_terminate_auth_session(dev, oiap_session.handle);
+		tpm1_terminate_auth_session(dev, oiap_session.handle);
 
 	err = tpm_sendrecv_command(dev, command, response, &response_length);
 	if (err)
@@ -702,9 +695,9 @@ u32 tpm_oiap(struct udevice *dev, u32 *auth_handle)
 	return 0;
 }
 
-u32 tpm_load_key2_oiap(struct udevice *dev, u32 parent_handle, const void *key,
-		       size_t key_length, const void *parent_key_usage_auth,
-		       u32 *key_handle)
+u32 tpm1_load_key2_oiap(struct udevice *dev, u32 parent_handle, const void *key,
+			size_t key_length, const void *parent_key_usage_auth,
+			u32 *key_handle)
 {
 	const u8 command[14] = {
 		0x00, 0xc2,		/* TPM_TAG */
@@ -723,7 +716,7 @@ u32 tpm_load_key2_oiap(struct udevice *dev, u32 parent_handle, const void *key,
 	u32 err;
 
 	if (!oiap_session.valid) {
-		err = tpm_oiap(dev, NULL);
+		err = tpm1_oiap(dev, NULL);
 		if (err)
 			return err;
 	}
@@ -768,9 +761,9 @@ u32 tpm_load_key2_oiap(struct udevice *dev, u32 parent_handle, const void *key,
 	return 0;
 }
 
-u32 tpm_get_pub_key_oiap(struct udevice *dev, u32 key_handle,
-			 const void *usage_auth, void *pubkey,
-			 size_t *pubkey_len)
+u32 tpm1_get_pub_key_oiap(struct udevice *dev, u32 key_handle,
+			  const void *usage_auth, void *pubkey,
+			  size_t *pubkey_len)
 {
 	const u8 command[14] = {
 		0x00, 0xc2,		/* TPM_TAG */
@@ -788,7 +781,7 @@ u32 tpm_get_pub_key_oiap(struct udevice *dev, u32 key_handle,
 	u32 err;
 
 	if (!oiap_session.valid) {
-		err = tpm_oiap(dev, NULL);
+		err = tpm1_oiap(dev, NULL);
 		if (err)
 			return err;
 	}
@@ -834,8 +827,8 @@ u32 tpm_get_pub_key_oiap(struct udevice *dev, u32 key_handle,
 }
 
 #ifdef CONFIG_TPM_LOAD_KEY_BY_SHA1
-u32 tpm_find_key_sha1(struct udevice *dev, const u8 auth[20],
-		      const u8 pubkey_digest[20], u32 *handle)
+u32 tpm1_find_key_sha1(struct udevice *dev, const u8 auth[20],
+		       const u8 pubkey_digest[20], u32 *handle)
 {
 	u16 key_count;
 	u32 key_handles[10];
@@ -876,7 +869,7 @@ u32 tpm_find_key_sha1(struct udevice *dev, const u8 auth[20],
 
 #endif /* CONFIG_TPM_AUTH_SESSIONS */
 
-u32 tpm_get_random(struct udevice *dev, void *data, u32 count)
+u32 tpm1_get_random(struct udevice *dev, void *data, u32 count)
 {
 	const u8 command[14] = {
 		0x0, 0xc1,		/* TPM_TAG */
diff --git a/lib/tpm-v2.c b/lib/tpm-v2.c
index 1f3deb06e4..235f8c20d4 100644
--- a/lib/tpm-v2.c
+++ b/lib/tpm-v2.c
@@ -47,9 +47,11 @@ u32 tpm2_self_test(struct udevice *dev, enum tpm2_yes_no full_test)
 u32 tpm2_clear(struct udevice *dev, u32 handle, const char *pw,
 	       const ssize_t pw_sz)
 {
+	/* Length of the message header, up to start of password */
+	uint offset = 27;
 	u8 command_v2[COMMAND_BUFFER_SIZE] = {
 		tpm_u16(TPM2_ST_SESSIONS),	/* TAG */
-		tpm_u32(27 + pw_sz),		/* Length */
+		tpm_u32(offset + pw_sz),	/* Length */
 		tpm_u32(TPM2_CC_CLEAR),		/* Command code */
 
 		/* HANDLE */
@@ -64,7 +66,6 @@ u32 tpm2_clear(struct udevice *dev, u32 handle, const char *pw,
 		tpm_u16(pw_sz),			/* Size of <hmac/password> */
 		/* STRING(pw)			   <hmac/password> (if any) */
 	};
-	unsigned int offset = 27;
 	int ret;
 
 	/*
@@ -80,12 +81,61 @@ u32 tpm2_clear(struct udevice *dev, u32 handle, const char *pw,
 	return tpm_sendrecv_command(dev, command_v2, NULL, NULL);
 }
 
+u32 tpm2_nv_define_space(struct udevice *dev, u32 space_index,
+			 size_t space_size, u32 nv_attributes,
+			 const u8 *nv_policy, size_t nv_policy_size)
+{
+	/*
+	 * Calculate the offset of the nv_policy piece by adding each of the
+	 * chunks below.
+	 */
+	uint offset = 10 + 8 + 13 + 14;
+	u8 command_v2[COMMAND_BUFFER_SIZE] = {
+		/* header 10 bytes */
+		tpm_u16(TPM2_ST_SESSIONS),	/* TAG */
+		tpm_u32(offset + nv_policy_size),/* Length */
+		tpm_u32(TPM2_CC_NV_DEFINE_SPACE),/* Command code */
+
+		/* handles 8 bytes */
+		tpm_u32(TPM2_RH_PLATFORM),	/* Primary platform seed */
+
+		/* session header 13 bytes */
+		tpm_u32(9),			/* Header size */
+		tpm_u32(TPM2_RS_PW),		/* Password authorisation */
+		tpm_u16(0),			/* nonce_size */
+		0,				/* session_attrs */
+		tpm_u16(0),			/* auth_size */
+
+		/* message 14 bytes + policy */
+		tpm_u16(12 + nv_policy_size),	/* size */
+		tpm_u32(space_index),
+		tpm_u16(TPM2_ALG_SHA256),
+		tpm_u32(nv_attributes),
+		tpm_u16(nv_policy_size),
+		/* nv_policy */
+	};
+	int ret;
+
+	/*
+	 * Fill the command structure starting from the first buffer:
+	 *     - the password (if any)
+	 */
+	ret = pack_byte_string(command_v2, sizeof(command_v2), "s",
+			       offset, nv_policy, nv_policy_size);
+	if (ret)
+		return TPM_LIB_ERROR;
+
+	return tpm_sendrecv_command(dev, command_v2, NULL, NULL);
+}
+
 u32 tpm2_pcr_extend(struct udevice *dev, u32 index, u32 algorithm,
 		    const u8 *digest, u32 digest_len)
 {
+	/* Length of the message header, up to start of digest */
+	uint offset = 33;
 	u8 command_v2[COMMAND_BUFFER_SIZE] = {
 		tpm_u16(TPM2_ST_SESSIONS),	/* TAG */
-		tpm_u32(33 + digest_len),	/* Length */
+		tpm_u32(offset + digest_len),	/* Length */
 		tpm_u32(TPM2_CC_PCR_EXTEND),	/* Command code */
 
 		/* HANDLE */
@@ -99,11 +149,12 @@ u32 tpm2_pcr_extend(struct udevice *dev, u32 index, u32 algorithm,
 		0,				/* Attributes: Cont/Excl/Rst */
 		tpm_u16(0),			/* Size of <hmac/password> */
 						/* <hmac/password> (if any) */
+
+		/* hashes */
 		tpm_u32(1),			/* Count (number of hashes) */
 		tpm_u16(algorithm),	/* Algorithm of the hash */
 		/* STRING(digest)		   Digest */
 	};
-	unsigned int offset = 33;
 	int ret;
 
 	/*
@@ -112,13 +163,96 @@ u32 tpm2_pcr_extend(struct udevice *dev, u32 index, u32 algorithm,
 	 */
 	ret = pack_byte_string(command_v2, sizeof(command_v2), "s",
 			       offset, digest, digest_len);
-	offset += digest_len;
 	if (ret)
 		return TPM_LIB_ERROR;
 
 	return tpm_sendrecv_command(dev, command_v2, NULL, NULL);
 }
 
+u32 tpm2_nv_read_value(struct udevice *dev, u32 index, void *data, u32 count)
+{
+	u8 command_v2[COMMAND_BUFFER_SIZE] = {
+		/* header 10 bytes */
+		tpm_u16(TPM2_ST_SESSIONS),	/* TAG */
+		tpm_u32(10 + 8 + 4 + 9 + 4),	/* Length */
+		tpm_u32(TPM2_CC_NV_READ),	/* Command code */
+
+		/* handles 8 bytes */
+		tpm_u32(TPM2_RH_PLATFORM),	/* Primary platform seed */
+		tpm_u32(HR_NV_INDEX + index),	/* Password authorisation */
+
+		/* AUTH_SESSION */
+		tpm_u32(9),			/* Authorization size */
+		tpm_u32(TPM2_RS_PW),		/* Session handle */
+		tpm_u16(0),			/* Size of <nonce> */
+						/* <nonce> (if any) */
+		0,				/* Attributes: Cont/Excl/Rst */
+		tpm_u16(0),			/* Size of <hmac/password> */
+						/* <hmac/password> (if any) */
+
+		tpm_u16(count),			/* Number of bytes */
+		tpm_u16(0),			/* Offset */
+	};
+	size_t response_len = COMMAND_BUFFER_SIZE;
+	u8 response[COMMAND_BUFFER_SIZE];
+	int ret;
+	u16 tag;
+	u32 size, code;
+
+	ret = tpm_sendrecv_command(dev, command_v2, response, &response_len);
+	if (ret)
+		return log_msg_ret("read", ret);
+	if (unpack_byte_string(response, response_len, "wdds",
+			       0, &tag, 2, &size, 6, &code,
+			       16, data, count))
+		return TPM_LIB_ERROR;
+
+	return 0;
+}
+
+u32 tpm2_nv_write_value(struct udevice *dev, u32 index, const void *data,
+			u32 count)
+{
+	struct tpm_chip_priv *priv = dev_get_uclass_priv(dev);
+	uint offset = 10 + 8 + 4 + 9 + 2;
+	uint len = offset + count + 2;
+	/* Use empty password auth if platform hierarchy is disabled */
+	u32 auth = priv->plat_hier_disabled ? HR_NV_INDEX + index :
+		TPM2_RH_PLATFORM;
+	u8 command_v2[COMMAND_BUFFER_SIZE] = {
+		/* header 10 bytes */
+		tpm_u16(TPM2_ST_SESSIONS),	/* TAG */
+		tpm_u32(len),			/* Length */
+		tpm_u32(TPM2_CC_NV_WRITE),	/* Command code */
+
+		/* handles 8 bytes */
+		tpm_u32(auth),			/* Primary platform seed */
+		tpm_u32(HR_NV_INDEX + index),	/* Password authorisation */
+
+		/* AUTH_SESSION */
+		tpm_u32(9),			/* Authorization size */
+		tpm_u32(TPM2_RS_PW),		/* Session handle */
+		tpm_u16(0),			/* Size of <nonce> */
+						/* <nonce> (if any) */
+		0,				/* Attributes: Cont/Excl/Rst */
+		tpm_u16(0),			/* Size of <hmac/password> */
+						/* <hmac/password> (if any) */
+
+		tpm_u16(count),
+	};
+	size_t response_len = COMMAND_BUFFER_SIZE;
+	u8 response[COMMAND_BUFFER_SIZE];
+	int ret;
+
+	ret = pack_byte_string(command_v2, sizeof(command_v2), "sw",
+			       offset, data, count,
+			       offset + count, 0);
+	if (ret)
+		return TPM_LIB_ERROR;
+
+	return tpm_sendrecv_command(dev, command_v2, response, &response_len);
+}
+
 u32 tpm2_pcr_read(struct udevice *dev, u32 idx, unsigned int idx_min_sz,
 		  void *data, unsigned int *updates)
 {
@@ -467,3 +601,61 @@ u32 tpm2_get_random(struct udevice *dev, void *data, u32 count)
 
 	return 0;
 }
+
+u32 tpm2_write_lock(struct udevice *dev, u32 index)
+{
+	u8 command_v2[COMMAND_BUFFER_SIZE] = {
+		/* header 10 bytes */
+		tpm_u16(TPM2_ST_SESSIONS),	/* TAG */
+		tpm_u32(10 + 8 + 13), /* Length */
+		tpm_u32(TPM2_CC_NV_WRITELOCK), /* Command code */
+
+		/* handles 8 bytes */
+		tpm_u32(TPM2_RH_PLATFORM),	/* Primary platform seed */
+		tpm_u32(HR_NV_INDEX + index),	/* Password authorisation */
+
+		/* session header 9 bytes */
+		tpm_u32(9),			/* Header size */
+		tpm_u32(TPM2_RS_PW),		/* Password authorisation */
+		tpm_u16(0),			/* nonce_size */
+		0,				/* session_attrs */
+		tpm_u16(0),			/* auth_size */
+	};
+
+	return tpm_sendrecv_command(dev, command_v2, NULL, NULL);
+}
+
+u32 tpm2_disable_platform_hierarchy(struct udevice *dev)
+{
+	struct tpm_chip_priv *priv = dev_get_uclass_priv(dev);
+	u8 command_v2[COMMAND_BUFFER_SIZE] = {
+		/* header 10 bytes */
+		tpm_u16(TPM2_ST_SESSIONS),	/* TAG */
+		tpm_u32(10 + 4 + 13 + 5),	/* Length */
+		tpm_u32(TPM2_CC_HIER_CONTROL),	/* Command code */
+
+		/* 4 bytes */
+		tpm_u32(TPM2_RH_PLATFORM),	/* Primary platform seed */
+
+		/* session header 9 bytes */
+		tpm_u32(9),			/* Header size */
+		tpm_u32(TPM2_RS_PW),		/* Password authorisation */
+		tpm_u16(0),			/* nonce_size */
+		0,				/* session_attrs */
+		tpm_u16(0),			/* auth_size */
+
+		/* payload 5 bytes */
+		tpm_u32(TPM2_RH_PLATFORM),	/* Hierarchy to disable */
+		0,				/* 0=disable */
+	};
+	int ret;
+
+	ret = tpm_sendrecv_command(dev, command_v2, NULL, NULL);
+	log_info("ret=%s, %x\n", dev->name, ret);
+	if (ret)
+		return ret;
+
+	priv->plat_hier_disabled = true;
+
+	return 0;
+}
diff --git a/lib/tpm_api.c b/lib/tpm_api.c
new file mode 100644
index 0000000000..4c662640a9
--- /dev/null
+++ b/lib/tpm_api.c
@@ -0,0 +1,285 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright 2019 Google LLC
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <log.h>
+#include <tpm_api.h>
+#include <tpm-v1.h>
+#include <tpm-v2.h>
+#include <tpm_api.h>
+
+static bool is_tpm1(struct udevice *dev)
+{
+	return IS_ENABLED(CONFIG_TPM_V1) && tpm_get_version(dev) == TPM_V1;
+}
+
+static bool is_tpm2(struct udevice *dev)
+{
+	return IS_ENABLED(CONFIG_TPM_V2) && tpm_get_version(dev) == TPM_V2;
+}
+
+u32 tpm_startup(struct udevice *dev, enum tpm_startup_type mode)
+{
+	if (is_tpm1(dev)) {
+		return tpm1_startup(dev, mode);
+	} else if (is_tpm2(dev)) {
+		enum tpm2_startup_types type;
+
+		switch (mode) {
+		case TPM_ST_CLEAR:
+			type = TPM2_SU_CLEAR;
+			break;
+		case TPM_ST_STATE:
+			type = TPM2_SU_STATE;
+			break;
+		default:
+		case TPM_ST_DEACTIVATED:
+			return -EINVAL;
+		}
+		return tpm2_startup(dev, type);
+	} else {
+		return -ENOSYS;
+	}
+}
+
+u32 tpm_resume(struct udevice *dev)
+{
+	if (is_tpm1(dev))
+		return tpm1_startup(dev, TPM_ST_STATE);
+	else if (is_tpm2(dev))
+		return tpm2_startup(dev, TPM2_SU_STATE);
+	else
+		return -ENOSYS;
+}
+
+u32 tpm_self_test_full(struct udevice *dev)
+{
+	if (is_tpm1(dev))
+		return tpm1_self_test_full(dev);
+	else if (is_tpm2(dev))
+		return tpm2_self_test(dev, TPMI_YES);
+	else
+		return -ENOSYS;
+}
+
+u32 tpm_continue_self_test(struct udevice *dev)
+{
+	if (is_tpm1(dev))
+		return tpm1_continue_self_test(dev);
+	else if (is_tpm2(dev))
+		return tpm2_self_test(dev, TPMI_NO);
+	else
+		return -ENOSYS;
+}
+
+u32 tpm_clear_and_reenable(struct udevice *dev)
+{
+	u32 ret;
+
+	log_info("TPM: Clear and re-enable\n");
+	ret = tpm_force_clear(dev);
+	if (ret != TPM_SUCCESS) {
+		log_err("Can't initiate a force clear\n");
+		return ret;
+	}
+
+	if (is_tpm1(dev)) {
+		ret = tpm1_physical_enable(dev);
+		if (ret != TPM_SUCCESS) {
+			log_err("TPM: Can't set enabled state\n");
+			return ret;
+		}
+
+		ret = tpm1_physical_set_deactivated(dev, 0);
+		if (ret != TPM_SUCCESS) {
+			log_err("TPM: Can't set deactivated state\n");
+			return ret;
+		}
+	}
+
+	return TPM_SUCCESS;
+}
+
+u32 tpm_nv_enable_locking(struct udevice *dev)
+{
+	if (is_tpm1(dev))
+		return tpm1_nv_define_space(dev, TPM_NV_INDEX_LOCK, 0, 0);
+	else if (is_tpm2(dev))
+		return -ENOSYS;
+	else
+		return -ENOSYS;
+}
+
+u32 tpm_nv_read_value(struct udevice *dev, u32 index, void *data, u32 count)
+{
+	if (is_tpm1(dev))
+		return tpm1_nv_read_value(dev, index, data, count);
+	else if (is_tpm2(dev))
+		return tpm2_nv_read_value(dev, index, data, count);
+	else
+		return -ENOSYS;
+}
+
+u32 tpm_nv_write_value(struct udevice *dev, u32 index, const void *data,
+		       u32 count)
+{
+	if (is_tpm1(dev))
+		return tpm1_nv_write_value(dev, index, data, count);
+	else if (is_tpm2(dev))
+		return tpm2_nv_write_value(dev, index, data, count);
+	else
+		return -ENOSYS;
+}
+
+u32 tpm_set_global_lock(struct udevice *dev)
+{
+	return tpm_nv_write_value(dev, TPM_NV_INDEX_0, NULL, 0);
+}
+
+u32 tpm_write_lock(struct udevice *dev, u32 index)
+{
+	if (is_tpm1(dev))
+		return -ENOSYS;
+	else if (is_tpm2(dev))
+		return tpm2_write_lock(dev, index);
+	else
+		return -ENOSYS;
+}
+
+u32 tpm_pcr_extend(struct udevice *dev, u32 index, const void *in_digest,
+		   void *out_digest)
+{
+	if (is_tpm1(dev))
+		return tpm1_extend(dev, index, in_digest, out_digest);
+	else if (is_tpm2(dev))
+		return tpm2_pcr_extend(dev, index, TPM2_ALG_SHA256, in_digest,
+				       TPM2_DIGEST_LEN);
+	else
+		return -ENOSYS;
+}
+
+u32 tpm_pcr_read(struct udevice *dev, u32 index, void *data, size_t count)
+{
+	if (is_tpm1(dev))
+		return tpm1_pcr_read(dev, index, data, count);
+	else if (is_tpm2(dev))
+		return -ENOSYS;
+	else
+		return -ENOSYS;
+}
+
+u32 tpm_tsc_physical_presence(struct udevice *dev, u16 presence)
+{
+	if (is_tpm1(dev))
+		return tpm1_tsc_physical_presence(dev, presence);
+
+	/*
+	 * Nothing to do on TPM2 for this; use platform hierarchy availability
+	 * instead.
+	 */
+	else if (is_tpm2(dev))
+		return 0;
+	else
+		return -ENOSYS;
+}
+
+u32 tpm_finalise_physical_presence(struct udevice *dev)
+{
+	if (is_tpm1(dev))
+		return tpm1_finalise_physical_presence(dev);
+
+	/* Nothing needs to be done with tpm2 */
+	else if (is_tpm2(dev))
+		return 0;
+	else
+		return -ENOSYS;
+}
+
+u32 tpm_read_pubek(struct udevice *dev, void *data, size_t count)
+{
+	if (is_tpm1(dev))
+		return tpm1_read_pubek(dev, data, count);
+	else if (is_tpm2(dev))
+		return -ENOSYS; /* not implemented yet */
+	else
+		return -ENOSYS;
+}
+
+u32 tpm_force_clear(struct udevice *dev)
+{
+	if (is_tpm1(dev))
+		return tpm1_force_clear(dev);
+	else if (is_tpm2(dev))
+		return tpm2_clear(dev, TPM2_RH_PLATFORM, NULL, 0);
+	else
+		return -ENOSYS;
+}
+
+u32 tpm_physical_enable(struct udevice *dev)
+{
+	if (is_tpm1(dev))
+		return tpm1_physical_enable(dev);
+
+	/* Nothing needs to be done with tpm2 */
+	else if (is_tpm2(dev))
+		return 0;
+	else
+		return -ENOSYS;
+}
+
+u32 tpm_physical_disable(struct udevice *dev)
+{
+	if (is_tpm1(dev))
+		return tpm1_physical_disable(dev);
+
+	/* Nothing needs to be done with tpm2 */
+	else if (is_tpm2(dev))
+		return 0;
+	else
+		return -ENOSYS;
+}
+
+u32 tpm_physical_set_deactivated(struct udevice *dev, u8 state)
+{
+	if (is_tpm1(dev))
+		return tpm1_physical_set_deactivated(dev, state);
+	/* Nothing needs to be done with tpm2 */
+	else if (is_tpm2(dev))
+		return 0;
+	else
+		return -ENOSYS;
+}
+
+u32 tpm_get_capability(struct udevice *dev, u32 cap_area, u32 sub_cap,
+		       void *cap, size_t count)
+{
+	if (is_tpm1(dev))
+		return tpm1_get_capability(dev, cap_area, sub_cap, cap, count);
+	else if (is_tpm2(dev))
+		return tpm2_get_capability(dev, cap_area, sub_cap, cap, count);
+	else
+		return -ENOSYS;
+}
+
+u32 tpm_get_permissions(struct udevice *dev, u32 index, u32 *perm)
+{
+	if (is_tpm1(dev))
+		return tpm1_get_permissions(dev, index, perm);
+	else if (is_tpm2(dev))
+		return -ENOSYS; /* not implemented yet */
+	else
+		return -ENOSYS;
+}
+
+u32 tpm_get_random(struct udevice *dev, void *data, u32 count)
+{
+	if (is_tpm1(dev))
+		return tpm1_get_random(dev, data, count);
+	else if (is_tpm2(dev))
+		return -ENOSYS; /* not implemented yet */
+	else
+		return -ENOSYS;
+}