diff --git a/lib/efi_selftest/Makefile b/lib/efi_selftest/Makefile
index 9ff6e1760c..09950ee028 100644
--- a/lib/efi_selftest/Makefile
+++ b/lib/efi_selftest/Makefile
@@ -14,6 +14,8 @@ CFLAGS_efi_selftest_miniapp_exception.o := $(CFLAGS_EFI) -Os -ffreestanding
 CFLAGS_REMOVE_efi_selftest_miniapp_exception.o := $(CFLAGS_NON_EFI)
 CFLAGS_efi_selftest_miniapp_exit.o := $(CFLAGS_EFI) -Os -ffreestanding
 CFLAGS_REMOVE_efi_selftest_miniapp_exit.o := $(CFLAGS_NON_EFI)
+CFLAGS_efi_selftest_miniapp_measuredboot.o := $(CFLAGS_EFI) -Os -ffreestanding
+CFLAGS_REMOVE_efi_selftest_miniapp_measuredboot.o := $(CFLAGS_NON_EFI)
 CFLAGS_efi_selftest_miniapp_return.o := $(CFLAGS_EFI) -Os -ffreestanding
 CFLAGS_REMOVE_efi_selftest_miniapp_return.o := $(CFLAGS_NON_EFI)
 CFLAGS_initrddump_exit.o := $(CFLAGS_EFI) -Os -ffreestanding
@@ -78,9 +80,11 @@ obj-$(CONFIG_EFI_ESRT) += efi_selftest_esrt.o
 targets += \
 efi_miniapp_file_image_exception.h \
 efi_miniapp_file_image_exit.h \
+efi_miniapp_file_image_measuredboot.h \
 efi_miniapp_file_image_return.h \
 efi_selftest_miniapp_exception.efi \
 efi_selftest_miniapp_exit.efi \
+efi_selftest_miniapp_measuredboot.efi \
 efi_selftest_miniapp_return.efi
 
 ifeq ($(CONFIG_GENERATE_ACPI_TABLE),)
@@ -99,6 +103,10 @@ $(obj)/efi_miniapp_file_image_exit.h: $(obj)/efi_selftest_miniapp_exit.efi
 	$(obj)/../../tools/file2include $(obj)/efi_selftest_miniapp_exit.efi > \
 	$(obj)/efi_miniapp_file_image_exit.h
 
+$(obj)/efi_miniapp_file_image_measuredboot.h: $(obj)/efi_selftest_miniapp_measuredboot.efi
+	$(obj)/../../tools/file2include $(obj)/efi_selftest_miniapp_measuredboot.efi > \
+	$(obj)/efi_miniapp_file_image_measuredboot.h
+
 $(obj)/efi_miniapp_file_image_return.h: $(obj)/efi_selftest_miniapp_return.efi
 	$(obj)/../../tools/file2include $(obj)/efi_selftest_miniapp_return.efi > \
 	$(obj)/efi_miniapp_file_image_return.h
@@ -112,3 +120,5 @@ $(obj)/efi_selftest_loadimage.o: $(obj)/efi_miniapp_file_image_exit.h
 $(obj)/efi_selftest_startimage_exit.o: $(obj)/efi_miniapp_file_image_exit.h
 
 $(obj)/efi_selftest_startimage_return.o: $(obj)/efi_miniapp_file_image_return.h
+
+$(obj)/efi_selftest_tcg2.o: $(obj)/efi_miniapp_file_image_measuredboot.h
diff --git a/lib/efi_selftest/efi_selftest_miniapp_measuredboot.c b/lib/efi_selftest/efi_selftest_miniapp_measuredboot.c
new file mode 100644
index 0000000000..926713c1c2
--- /dev/null
+++ b/lib/efi_selftest/efi_selftest_miniapp_measuredboot.c
@@ -0,0 +1,93 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * efi_selftest_miniapp_measuredboot
+ *
+ * Copyright (c) 2018 Heinrich Schuchardt
+ * Copyright (c) 2021 Masahisa Kojima
+ *
+ * This EFI application is run by the StartImage selftest.
+ * It uses the Exit boot service to return and used for
+ * Measured Boot selftest.
+ */
+
+#include <common.h>
+#include <efi_selftest.h>
+
+static efi_guid_t loaded_image_protocol_guid = EFI_LOADED_IMAGE_PROTOCOL_GUID;
+
+/**
+ * check_loaded_image_protocol() - check image_base/image_size
+ *
+ * Try to open the loaded image protocol. Check that this function is located
+ * between image_base and image_base + image_size.
+ *
+ * @image_handle:	handle of the loaded image
+ * @systable:		system table
+ * @return:		status code
+ */
+static efi_status_t EFIAPI check_loaded_image_protocol
+		(efi_handle_t image_handle, struct efi_system_table *systable)
+{
+	struct efi_simple_text_output_protocol *cout = systable->con_out;
+	struct efi_boot_services *boottime = systable->boottime;
+	struct efi_loaded_image *loaded_image_protocol;
+	efi_status_t ret;
+
+	/*
+	 * Open the loaded image protocol.
+	 */
+	ret = boottime->open_protocol
+				(image_handle, &loaded_image_protocol_guid,
+				 (void **)&loaded_image_protocol, NULL,
+				  NULL, EFI_OPEN_PROTOCOL_GET_PROTOCOL);
+	if (ret != EFI_SUCCESS) {
+		cout->output_string(cout,
+				    L"Could not open loaded image protocol");
+		return ret;
+	}
+	if ((void *)check_loaded_image_protocol <
+	    loaded_image_protocol->image_base ||
+	    (void *)check_loaded_image_protocol >=
+	    loaded_image_protocol->image_base +
+	    loaded_image_protocol->image_size) {
+		cout->output_string(cout,
+				    L"Incorrect image_base or image_size\n");
+		return EFI_NOT_FOUND;
+	}
+	return EFI_SUCCESS;
+}
+
+/**
+ * Entry point of the EFI application.
+ *
+ * @handle:	handle of the loaded image
+ * @systable:	system table
+ * @return:	status code
+ */
+efi_status_t EFIAPI efi_main(efi_handle_t handle,
+			     struct efi_system_table *systable)
+{
+	struct efi_simple_text_output_protocol *con_out = systable->con_out;
+	efi_status_t ret;
+	u16 text[] = EFI_ST_SUCCESS_STR;
+
+	con_out->output_string(con_out, L"EFI application calling Exit\n");
+
+	if (check_loaded_image_protocol(handle, systable) != EFI_SUCCESS) {
+		con_out->output_string(con_out,
+				       L"Loaded image protocol missing\n");
+		ret = EFI_NOT_FOUND;
+		goto out;
+	}
+
+	/* This return value is expected by the calling test */
+	ret = EFI_UNSUPPORTED;
+out:
+	systable->boottime->exit(handle, ret, sizeof(text), text);
+
+	/*
+	 * This statement should not be reached.
+	 * To enable testing use a different return value.
+	 */
+	return EFI_SUCCESS;
+}
diff --git a/lib/efi_selftest/efi_selftest_tcg2.c b/lib/efi_selftest/efi_selftest_tcg2.c
index 1399309cec..50de735f5e 100644
--- a/lib/efi_selftest/efi_selftest_tcg2.c
+++ b/lib/efi_selftest/efi_selftest_tcg2.c
@@ -9,10 +9,495 @@
 
 #include <efi_selftest.h>
 #include <efi_tcg2.h>
+/* Include containing the miniapp.efi application */
+#include "efi_miniapp_file_image_measuredboot.h"
+
+#include <linux/unaligned/access_ok.h>
+#include <mapmem.h>
+#include <smbios.h>
+#include <tables_csum.h>
 
 static struct efi_boot_services *boottime;
 static const efi_guid_t guid_tcg2 = EFI_TCG2_PROTOCOL_GUID;
 
+/* Block size of compressed disk image */
+#define COMPRESSED_DISK_IMAGE_BLOCK_SIZE 8
+
+static efi_handle_t image_handle;
+/* Decompressed file image */
+static u8 *image;
+
+/* One 8 byte block of the compressed disk image */
+struct line {
+	size_t addr;
+	char *line;
+};
+
+/* Compressed file image */
+struct compressed_file_image {
+	size_t length;
+	struct line lines[];
+};
+
+static struct compressed_file_image img = EFI_ST_DISK_IMG;
+
+static struct efi_tcg2_event *efi_tcg2_event;
+
+static struct efi_runtime_services *runtime;
+#define BOOT_NAME_1000 u"Boot1000"
+#define BOOT_NAME_1001 u"Boot1001"
+#define BOOT_NAME_1002 u"Boot1002"
+
+#define DEFAULT_ATTR (EFI_VARIABLE_NON_VOLATILE | \
+		      EFI_VARIABLE_BOOTSERVICE_ACCESS | \
+		      EFI_VARIABLE_RUNTIME_ACCESS)
+
+/* "efidebug boot add -b 1000 test1000 virtio 0:1 /EFI/debian/grubaa64.efi" */
+static const u8 boot_1000[] = {
+0x01, 0x00, 0x00, 0x00, 0x8d, 0x00, 0x74, 0x00, 0x65, 0x00, 0x73, 0x00,
+0x74, 0x00, 0x30, 0x00, 0x00, 0x00, 0x01, 0x04, 0x14, 0x00, 0xb9, 0x73,
+0x1d, 0xe6, 0x84, 0xa3, 0xcc, 0x4a, 0xae, 0xab, 0x82, 0xe8, 0x28, 0xf3,
+0x62, 0x8b, 0x01, 0x04, 0x15, 0x00, 0x92, 0x37, 0x29, 0x63, 0xf5, 0xad,
+0x25, 0x93, 0xb9, 0x9f, 0x4e, 0x0e, 0x45, 0x5c, 0x1b, 0x1e, 0x00, 0x04,
+0x01, 0x2a, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x57,
+0x5a, 0x47, 0xc2, 0x35, 0x27, 0x44, 0x47, 0x9f, 0x01, 0x67, 0xfe, 0xfa,
+0x1d, 0x06, 0xae, 0x02, 0x02, 0x04, 0x04, 0x36, 0x00, 0x5c, 0x00, 0x45,
+0x00, 0x46, 0x00, 0x49, 0x00, 0x5c, 0x00, 0x64, 0x00, 0x65, 0x00, 0x62,
+0x00, 0x69, 0x00, 0x61, 0x00, 0x6e, 0x00, 0x5c, 0x00, 0x67, 0x00, 0x72,
+0x00, 0x75, 0x00, 0x62, 0x00, 0x61, 0x00, 0x61, 0x00, 0x36, 0x00, 0x34,
+0x00, 0x2e, 0x00, 0x65, 0x00, 0x66, 0x00, 0x69, 0x00, 0x00, 0x00, 0x7f,
+0xff, 0x04, 0x00 };
+
+/* "efidebug boot add -b 1001 test1001 virtio 0:1 /EFI/debian/grubaa64.efi" */
+static const u8 boot_1001[] = {
+0x01, 0x00, 0x00, 0x00, 0x8d, 0x00, 0x74, 0x00, 0x65, 0x00, 0x73, 0x00,
+0x74, 0x00, 0x31, 0x00, 0x00, 0x00, 0x01, 0x04, 0x14, 0x00, 0xb9, 0x73,
+0x1d, 0xe6, 0x84, 0xa3, 0xcc, 0x4a, 0xae, 0xab, 0x82, 0xe8, 0x28, 0xf3,
+0x62, 0x8b, 0x01, 0x04, 0x15, 0x00, 0x92, 0x37, 0x29, 0x63, 0xf5, 0xad,
+0x25, 0x93, 0xb9, 0x9f, 0x4e, 0x0e, 0x45, 0x5c, 0x1b, 0x1e, 0x00, 0x04,
+0x01, 0x2a, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x57,
+0x5a, 0x47, 0xc2, 0x35, 0x27, 0x44, 0x47, 0x9f, 0x01, 0x67, 0xfe, 0xfa,
+0x1d, 0x06, 0xae, 0x02, 0x02, 0x04, 0x04, 0x36, 0x00, 0x5c, 0x00, 0x45,
+0x00, 0x46, 0x00, 0x49, 0x00, 0x5c, 0x00, 0x64, 0x00, 0x65, 0x00, 0x62,
+0x00, 0x69, 0x00, 0x61, 0x00, 0x6e, 0x00, 0x5c, 0x00, 0x67, 0x00, 0x72,
+0x00, 0x75, 0x00, 0x62, 0x00, 0x61, 0x00, 0x61, 0x00, 0x36, 0x00, 0x34,
+0x00, 0x2e, 0x00, 0x65, 0x00, 0x66, 0x00, 0x69, 0x00, 0x00, 0x00, 0x7f,
+0xff, 0x04, 0x00 };
+
+/* "efidebug boot add -b 1002 test1002 virtio 0:1 /EFI/debian/grubaa64.efi" */
+static const u8 boot_1002[] = {
+0x01, 0x00, 0x00, 0x00, 0x8d, 0x00, 0x74, 0x00, 0x65, 0x00, 0x73, 0x00,
+0x74, 0x00, 0x32, 0x00, 0x00, 0x00, 0x01, 0x04, 0x14, 0x00, 0xb9, 0x73,
+0x1d, 0xe6, 0x84, 0xa3, 0xcc, 0x4a, 0xae, 0xab, 0x82, 0xe8, 0x28, 0xf3,
+0x62, 0x8b, 0x01, 0x04, 0x15, 0x00, 0x92, 0x37, 0x29, 0x63, 0xf5, 0xad,
+0x25, 0x93, 0xb9, 0x9f, 0x4e, 0x0e, 0x45, 0x5c, 0x1b, 0x1e, 0x00, 0x04,
+0x01, 0x2a, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x57,
+0x5a, 0x47, 0xc2, 0x35, 0x27, 0x44, 0x47, 0x9f, 0x01, 0x67, 0xfe, 0xfa,
+0x1d, 0x06, 0xae, 0x02, 0x02, 0x04, 0x04, 0x36, 0x00, 0x5c, 0x00, 0x45,
+0x00, 0x46, 0x00, 0x49, 0x00, 0x5c, 0x00, 0x64, 0x00, 0x65, 0x00, 0x62,
+0x00, 0x69, 0x00, 0x61, 0x00, 0x6e, 0x00, 0x5c, 0x00, 0x67, 0x00, 0x72,
+0x00, 0x75, 0x00, 0x62, 0x00, 0x61, 0x00, 0x61, 0x00, 0x36, 0x00, 0x34,
+0x00, 0x2e, 0x00, 0x65, 0x00, 0x66, 0x00, 0x69, 0x00, 0x00, 0x00, 0x7f,
+0xff, 0x04, 0x00};
+
+/* "efidebug boot order 1002 1000 1001" */
+static u8 boot_order[] = {0x02, 0x10, 0x00, 0x10, 0x01, 0x10};
+
+static void *orig_smbios_table;
+static u64 dmi_addr = U32_MAX;
+#define SMBIOS_ENTRY_HEADER_SIZE 0x20
+/* smbios table for the measurement test */
+static u8 smbios_table_test[] = {
+0x5f, 0x53, 0x4d, 0x5f, 0x2c, 0x1f, 0x03, 0x00, 0x54, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x5f, 0x44, 0x4d, 0x49, 0x5f, 0xe4, 0x5c, 0x01,
+0x20, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x18, 0x00, 0x00,
+0x01, 0x02, 0x00, 0x00, 0x03, 0x00, 0x80, 0x08, 0x01, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x0c, 0x15, 0x0a, 0xff, 0xff, 0x55, 0x2d, 0x42, 0x6f,
+0x6f, 0x74, 0x00, 0x32, 0x30, 0x32, 0x31, 0x2e, 0x31, 0x30, 0x2d, 0x72,
+0x63, 0x34, 0x2d, 0x30, 0x30, 0x30, 0x30, 0x35, 0x2d, 0x67, 0x37, 0x32,
+0x37, 0x63, 0x33, 0x66, 0x33, 0x32, 0x35, 0x39, 0x2d, 0x64, 0x69, 0x72,
+0x74, 0x79, 0x00, 0x31, 0x30, 0x2f, 0x30, 0x31, 0x2f, 0x32, 0x30, 0x32,
+0x31, 0x00, 0x00, 0x01, 0x1b, 0x01, 0x00, 0x01, 0x02, 0x00, 0x03, 0x31,
+0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x55, 0x6e, 0x6b, 0x6e, 0x6f, 0x77,
+0x6e, 0x00, 0x55, 0x6e, 0x6b, 0x6e, 0x6f, 0x77, 0x6e, 0x20, 0x50, 0x72,
+0x6f, 0x64, 0x75, 0x63, 0x74, 0x00, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36,
+0x37, 0x38, 0x00, 0x00, 0x02, 0x0e, 0x02, 0x00, 0x01, 0x02, 0x00, 0x04,
+0x03, 0x01, 0x01, 0x01, 0x00, 0x0a, 0x55, 0x6e, 0x6b, 0x6e, 0x6f, 0x77,
+0x6e, 0x00, 0x55, 0x6e, 0x6b, 0x6e, 0x6f, 0x77, 0x6e, 0x20, 0x50, 0x72,
+0x6f, 0x64, 0x75, 0x63, 0x74, 0x00, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33,
+0x33, 0x33, 0x00, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0x00,
+0x00, 0x03, 0x15, 0x03, 0x00, 0x01, 0x03, 0x00, 0x02, 0x03, 0x03, 0x03,
+0x03, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x55, 0x6e,
+0x6b, 0x6e, 0x6f, 0x77, 0x6e, 0x00, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36,
+0x37, 0x38, 0x00, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x00,
+0x00, 0x04, 0x30, 0x04, 0x00, 0x00, 0x03, 0x02, 0x01, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x0c, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x01, 0x06, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x02, 0x03, 0x04,
+0x04, 0x04, 0x08, 0x00, 0x00, 0x02, 0x00, 0x08, 0x00, 0x08, 0x00, 0x01,
+0x00, 0x55, 0x6e, 0x6b, 0x6e, 0x6f, 0x77, 0x6e, 0x00, 0x31, 0x32, 0x33,
+0x34, 0x35, 0x36, 0x37, 0x38, 0x00, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33,
+0x33, 0x33, 0x00, 0x35, 0x35, 0x35, 0x35, 0x35, 0x35, 0x35, 0x35, 0x00,
+0x00, 0x20, 0x0b, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x7f, 0x04, 0x06, 0x00, 0x00, 0x00
+};
+
+#define TPM2_CMD_BUF_SIZE 64
+/* TPM command is big endian */
+#define __MSB(x) ((x) >> 8)
+#define __LSB(x) ((x) & 0xFF)
+#define tpm_u16(x) __MSB(x), __LSB(x)
+#define tpm_u32(x) tpm_u16((x) >> 16), tpm_u16((x) & 0xFFFF)
+#define TPM2_PCR_READ_HEADER_SIZE 30
+
+static u8 *pcrs;
+static u8 expected_pcrs[EFI_TCG2_MAX_PCR_INDEX + 1][TPM2_SHA256_DIGEST_SIZE] = {
+	{0x91, 0x21, 0x37, 0xc7, 0x1a, 0x49, 0x19, 0xc8,
+	 0xf1, 0xfb, 0xa9, 0x84, 0x5c, 0x65, 0xa9, 0xdd,
+	 0x7b, 0xb9, 0xfe, 0xa1, 0xcd, 0x64, 0x49, 0xdd,
+	 0xed, 0xe2, 0x65, 0x82, 0xc5, 0x3e, 0xf4, 0xc4},
+
+	{0xf5, 0x79, 0xf3, 0x20, 0x62, 0x6e, 0x8b, 0x58,
+	 0x62, 0xa3, 0x4e, 0x2f, 0xb7, 0x10, 0xac, 0x34,
+	 0x4e, 0x68, 0x94, 0x37, 0x87, 0x29, 0xc4, 0xbe,
+	 0xa3, 0xc4, 0xd9, 0x14, 0x2b, 0x66, 0x79, 0x9b},
+
+	{0x3d, 0x45, 0x8c, 0xfe, 0x55, 0xcc, 0x03, 0xea,
+	 0x1f, 0x44, 0x3f, 0x15, 0x62, 0xbe, 0xec, 0x8d,
+	 0xf5, 0x1c, 0x75, 0xe1, 0x4a, 0x9f, 0xcf, 0x9a,
+	 0x72, 0x34, 0xa1, 0x3f, 0x19, 0x8e, 0x79, 0x69},
+
+	{0x3d, 0x45, 0x8c, 0xfe, 0x55, 0xcc, 0x03, 0xea,
+	 0x1f, 0x44, 0x3f, 0x15, 0x62, 0xbe, 0xec, 0x8d,
+	 0xf5, 0x1c, 0x75, 0xe1, 0x4a, 0x9f, 0xcf, 0x9a,
+	 0x72, 0x34, 0xa1, 0x3f, 0x19, 0x8e, 0x79, 0x69},
+
+	{0xbe, 0xea, 0xdc, 0xe0, 0x44, 0x5b, 0x5f, 0x14,
+	 0xef, 0x24, 0x5d, 0x13, 0x15, 0xfe, 0x41, 0x86,
+	 0xc2, 0xd5, 0xdc, 0x0d, 0x04, 0x2f, 0xd4, 0x04,
+	 0x0d, 0x02, 0x62, 0xc0, 0x34, 0x80, 0xee, 0xd5},
+
+	{0x3d, 0x45, 0x8c, 0xfe, 0x55, 0xcc, 0x03, 0xea,
+	 0x1f, 0x44, 0x3f, 0x15, 0x62, 0xbe, 0xec, 0x8d,
+	 0xf5, 0x1c, 0x75, 0xe1, 0x4a, 0x9f, 0xcf, 0x9a,
+	 0x72, 0x34, 0xa1, 0x3f, 0x19, 0x8e, 0x79, 0x69},
+
+	{0x8d, 0x28, 0xde, 0x72, 0x22, 0x3e, 0x88, 0x1a,
+	 0x37, 0xfa, 0x47, 0x12, 0x68, 0x45, 0xdf, 0x71,
+	 0x50, 0x8f, 0xab, 0x59, 0x50, 0x7b, 0x52, 0x32,
+	 0xa6, 0xaa, 0x03, 0x3d, 0x4e, 0x22, 0x89, 0xd7},
+
+	{0x96, 0x74, 0xae, 0xcd, 0x3f, 0x40, 0xb4, 0xa9,
+	 0x36, 0xae, 0x19, 0xc8, 0x84, 0x8a, 0xb9, 0x5a,
+	 0x87, 0x99, 0xd8, 0x89, 0x7f, 0xfc, 0x40, 0x48,
+	 0x05, 0x99, 0x65, 0x2e, 0x55, 0xd4, 0x93, 0x32},
+
+	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+
+	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+
+	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+
+	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+
+	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+
+	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+
+	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+
+	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+
+	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+
+	{0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff},
+
+	{0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff},
+
+	{0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff},
+
+	{0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff},
+
+	{0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff},
+
+	{0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+	 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff},
+
+	{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+};
+
+struct boot_variable {
+	u16 name[16];
+	u8 *buf;
+	efi_uintn_t size;
+	u32 attr;
+	const u8 *test_data;
+	efi_uintn_t test_data_size;
+};
+
+static struct boot_variable boot_variable_test[] = {
+	{u"BootOrder",		NULL, 0, DEFAULT_ATTR, boot_order, sizeof(boot_order)},
+	{BOOT_NAME_1000,	NULL, 0, DEFAULT_ATTR, boot_1000, sizeof(boot_1000)},
+	{BOOT_NAME_1001,	NULL, 0, DEFAULT_ATTR, boot_1001, sizeof(boot_1001)},
+	{BOOT_NAME_1002,	NULL, 0, DEFAULT_ATTR, boot_1002, sizeof(boot_1002)},
+};
+
+/*
+ * Decompress the disk image.
+ *
+ * @image	decompressed disk image
+ * @return	status code
+ */
+static efi_status_t decompress(u8 **image)
+{
+	u8 *buf;
+	size_t i;
+	size_t addr;
+	size_t len;
+	efi_status_t ret;
+
+	ret = boottime->allocate_pool(EFI_LOADER_DATA, img.length,
+				      (void **)&buf);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Out of memory\n");
+		return ret;
+	}
+	boottime->set_mem(buf, img.length, 0);
+
+	for (i = 0; ; ++i) {
+		if (!img.lines[i].line)
+			break;
+		addr = img.lines[i].addr;
+		len = COMPRESSED_DISK_IMAGE_BLOCK_SIZE;
+		if (addr + len > img.length)
+			len = img.length - addr;
+		boottime->copy_mem(buf + addr, img.lines[i].line, len);
+	}
+	*image = buf;
+	return ret;
+}
+
+/*
+ * Configure dummy boot variables.
+ *
+ * @return	status code
+ */
+static efi_status_t setup_boot_variable(void)
+{
+	efi_status_t ret;
+	u32 i;
+	efi_uintn_t size;
+	u8 dummy;
+
+	for (i = 0; i < ARRAY_SIZE(boot_variable_test); i++) {
+		size = 1;
+		ret = runtime->get_variable(boot_variable_test[i].name,
+					    &efi_global_variable_guid,
+					    &boot_variable_test[i].attr,
+					    &size,
+					    &dummy);
+		if (ret == EFI_BUFFER_TOO_SMALL) {
+			/* Variable exists, save the current vaiable */
+			boot_variable_test[i].size = size;
+			ret = boottime->allocate_pool(EFI_LOADER_DATA,
+						      boot_variable_test[i].size,
+						      (void **)&boot_variable_test[i].buf);
+			if (ret != EFI_SUCCESS) {
+				efi_st_error("fail to allocate buffer for boot variable\n");
+				return ret;
+			}
+			ret = runtime->get_variable(boot_variable_test[i].name,
+						    &efi_global_variable_guid,
+						    &boot_variable_test[i].attr,
+						    &boot_variable_test[i].size,
+						    boot_variable_test[i].buf);
+			if (ret != EFI_SUCCESS) {
+				efi_st_error("fail to get current boot variable\n");
+				return ret;
+			}
+		}
+
+		/* set boot variable for the measurement test */
+		ret = runtime->set_variable(boot_variable_test[i].name,
+					    &efi_global_variable_guid,
+					    boot_variable_test[i].attr,
+					    boot_variable_test[i].test_data_size,
+					    boot_variable_test[i].test_data);
+		if (ret != EFI_SUCCESS) {
+			efi_st_error("### fail to set test boot variable(%d)n", i);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * Restore original boot variables.
+ *
+ * @return	status code
+ */
+efi_status_t restore_boot_variable(void)
+{
+	int i;
+	efi_status_t ret;
+
+	for (i = 0; i < ARRAY_SIZE(boot_variable_test); i++) {
+		if (boot_variable_test[i].buf) {
+			ret = runtime->set_variable(boot_variable_test[i].name,
+						    &efi_global_variable_guid,
+						    boot_variable_test[i].attr,
+						    boot_variable_test[i].size,
+						    boot_variable_test[i].buf);
+			if (ret != EFI_SUCCESS) {
+				efi_st_error("### fail to restore boot variable\n");
+				return ret;
+			}
+			ret = boottime->free_pool(boot_variable_test[i].buf);
+			if (ret != EFI_SUCCESS) {
+				efi_st_error("Failed to free boot variable\n");
+				return ret;
+			}
+		} else {
+			/* delete the variable used only for testing */
+			ret = runtime->set_variable(boot_variable_test[i].name,
+						    &efi_global_variable_guid,
+						    0, 0, NULL);
+			if (ret != EFI_SUCCESS) {
+				efi_st_error("### fail to delete boot variable\n");
+				return ret;
+			}
+		}
+	}
+
+	return EFI_SUCCESS;
+}
+
+/**
+ * Find smbios table
+ *
+ * @systable	system table
+ * @return	status code
+ */
+static void *find_smbios_table(const struct efi_system_table *systable)
+{
+	u32 i;
+
+	for (i = 0; i < systable->nr_tables; i++) {
+		if (!guidcmp(&smbios_guid, &systable->tables[i].guid))
+			return systable->tables[i].table;
+	}
+
+	return NULL;
+}
+
+/**
+ * Prepare the dummy SMBIOS table
+ *
+ * @systable	system table
+ * @return	status code
+ */
+efi_status_t setup_smbios_table(const struct efi_system_table *systable)
+{
+	struct smbios_entry *se;
+	efi_status_t ret;
+	/* Map within the low 32 bits, to allow for 32bit SMBIOS tables */
+	void *dmi;
+	char *istart;
+	int isize;
+
+	if (sizeof(smbios_table_test) > EFI_PAGE_SIZE)
+		return EFI_OUT_OF_RESOURCES;
+
+	orig_smbios_table = find_smbios_table(systable);
+
+	/* Reserve 4kiB page for SMBIOS */
+	ret = boottime->allocate_pages(EFI_ALLOCATE_MAX_ADDRESS,
+				 EFI_RUNTIME_SERVICES_DATA, 1, &dmi_addr);
+
+	if (ret != EFI_SUCCESS) {
+		/* Could not find space in lowmem, use highmem instead */
+		ret = boottime->allocate_pages(EFI_ALLOCATE_ANY_PAGES,
+					 EFI_RUNTIME_SERVICES_DATA, 1,
+					 &dmi_addr);
+
+		if (ret != EFI_SUCCESS)
+			return ret;
+	}
+
+	dmi = (void *)(uintptr_t)dmi_addr;
+	se = dmi;
+	boottime->copy_mem(se, smbios_table_test, sizeof(smbios_table_test));
+
+	/* update smbios table start address */
+	se->struct_table_address = (uintptr_t)((u8 *)dmi + SMBIOS_ENTRY_HEADER_SIZE);
+
+	/* calculate checksums */
+	istart = (char *)se + SMBIOS_INTERMEDIATE_OFFSET;
+	isize = sizeof(struct smbios_entry) - SMBIOS_INTERMEDIATE_OFFSET;
+	se->intermediate_checksum = table_compute_checksum(istart, isize);
+	se->checksum = table_compute_checksum(se, sizeof(struct smbios_entry));
+
+	/* Install SMBIOS information as configuration table */
+	ret = boottime->install_configuration_table(&smbios_guid, dmi);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Cannot install SMBIOS table\n");
+		boottime->free_pages(dmi_addr, 1);
+	}
+
+	return ret;
+}
+
 /**
  * efi_st_tcg2_setup() - setup test
  *
@@ -23,7 +508,171 @@ static const efi_guid_t guid_tcg2 = EFI_TCG2_PROTOCOL_GUID;
 static int efi_st_tcg2_setup(const efi_handle_t img_handle,
 			     const struct efi_system_table *systable)
 {
+	efi_status_t ret;
+	struct uefi_image_load_event image_load_event;
+
+	image_handle = img_handle;
 	boottime = systable->boottime;
+	runtime = systable->runtime;
+
+	/* Load the application image into memory */
+	decompress(&image);
+
+	ret = boottime->allocate_pool(EFI_LOADER_DATA,
+				      sizeof(struct efi_tcg2_event) +
+				      sizeof(struct uefi_image_load_event),
+				      (void **)&efi_tcg2_event);
+	if (!efi_tcg2_event)
+		return EFI_ST_FAILURE;
+
+	efi_tcg2_event->size = sizeof(struct efi_tcg2_event) +
+			       sizeof(struct uefi_image_load_event);
+	efi_tcg2_event->header.header_size = sizeof(struct efi_tcg2_event_header);
+	efi_tcg2_event->header.header_version = 1;
+	efi_tcg2_event->header.pcr_index = 6;
+	efi_tcg2_event->header.event_type = EV_EFI_RUNTIME_SERVICES_DRIVER;
+	image_load_event.image_location_in_memory = 0x12345678;
+	image_load_event.image_length_in_memory = 0x300000;
+	image_load_event.image_link_time_address = 0x87654321;
+	image_load_event.length_of_device_path = 0;
+	boottime->copy_mem(efi_tcg2_event->event, &image_load_event,
+			   sizeof(struct uefi_image_load_event));
+
+	ret = setup_boot_variable();
+	if (ret != EFI_SUCCESS)
+		return EFI_ST_FAILURE;
+
+	ret = setup_smbios_table(systable);
+	if (ret != EFI_SUCCESS)
+		return EFI_ST_FAILURE;
+
+	ret = boottime->allocate_pool(EFI_LOADER_DATA,
+				      (EFI_TCG2_MAX_PCR_INDEX + 1) *
+				      TPM2_SHA256_DIGEST_SIZE,
+				      (void **)&pcrs);
+	if (!pcrs)
+		return EFI_ST_FAILURE;
+
+	boottime->set_mem(pcrs, (EFI_TCG2_MAX_PCR_INDEX + 1) * TPM2_SHA256_DIGEST_SIZE, 0);
+
+	return EFI_ST_SUCCESS;
+}
+
+/**
+ * Get manufacturer_id through submit_command API
+ *
+ * @tcg2		tcg2 protocol
+ * @manufacturer_id	pointer to the manufacturer_id
+ * @return		status code
+ */
+static efi_status_t get_manufacturer_id(struct efi_tcg2_protocol *tcg2, u32 *manufacturer_id)
+{
+	efi_status_t ret;
+	u8 cmd[TPM2_CMD_BUF_SIZE] = {
+		tpm_u16(TPM2_ST_NO_SESSIONS),		/* TAG */
+		tpm_u32(22),				/* Length */
+		tpm_u32(TPM2_CC_GET_CAPABILITY),	/* Command code */
+
+		tpm_u32(TPM2_CAP_TPM_PROPERTIES),	/* Capability */
+		tpm_u32(TPM2_PT_MANUFACTURER),		/* Property */
+		tpm_u32(1),			/* Property count */
+	};
+	u8 resp[TPM2_CMD_BUF_SIZE];
+	unsigned int value_off;
+
+	ret = tcg2->submit_command(tcg2, 22, cmd,
+				   TPM2_CMD_BUF_SIZE, resp);
+	if (ret != EFI_SUCCESS)
+		return ret;
+
+	/*
+	 * In the response buffer, the properties are located after the:
+	 * tag (u16), response size (u32), response code (u32),
+	 * YES/NO flag (u8), TPM_CAP (u32).
+	 * The value is located after count (u32), property (u32).
+	 */
+	value_off = sizeof(u16) + sizeof(u32) + sizeof(u32) +
+			 sizeof(u8) + sizeof(u32) + sizeof(u32) + sizeof(u32);
+	*manufacturer_id = get_unaligned_be32(&resp[value_off]);
+
+	return ret;
+}
+
+/**
+ * Read the PCR from the TPM device
+ *
+ * @tcg2	tcg2 protocol
+ * @idx		pcr index to read
+ * @return	status code
+ */
+static efi_status_t read_pcr(struct efi_tcg2_protocol *tcg2, u32 idx)
+{
+	efi_status_t ret;
+	u8 idx_array_sz = 3; /* support 24 PCRs */
+	u32 cmd_len = 17 + idx_array_sz;
+	u8 cmd[TPM2_CMD_BUF_SIZE] = {
+		tpm_u16(TPM2_ST_NO_SESSIONS),	/* TAG */
+		tpm_u32(cmd_len),		/* Length */
+		tpm_u32(TPM2_CC_PCR_READ),	/* Command code */
+		/* TPML_PCR_SELECTION */
+		tpm_u32(1),			/* Number of selections */
+		tpm_u16(TPM2_ALG_SHA256),	/* Algorithm of the hash */
+		idx_array_sz,			/* Array size for selection */
+		/* bitmap(idx),			   Selected PCR bitmap */
+	};
+	u8 resp[TPM2_CMD_BUF_SIZE];
+	u32 pcr_sel_idx = idx / 8;
+	u8 pcr_sel_bit = BIT(idx % 8);
+	u8 *dst;
+
+	cmd[17 + pcr_sel_idx] = pcr_sel_bit;
+	ret = tcg2->submit_command(tcg2, cmd_len, cmd,
+				   TPM2_CMD_BUF_SIZE, resp);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("tcg2->submit_command fail to read PCR\n");
+		return ret;
+	}
+
+	dst = pcrs + (idx * TPM2_SHA256_DIGEST_SIZE);
+	boottime->copy_mem(dst, &resp[TPM2_PCR_READ_HEADER_SIZE],
+			   TPM2_SHA256_DIGEST_SIZE);
+
+	return ret;
+}
+
+/**
+ * Compare the expected and actual pcrs
+ *
+ * @return	status code
+ */
+static int validate_pcrs(void)
+{
+	u32 i;
+	u8 *expected = (u8 *)expected_pcrs;
+	u8 *result = pcrs;
+
+	/*
+	 *  - Skip PCR[0] validation. PCR[0] contains U-Boot version measurement
+	 *    it contains the commit hash, so the measurement varies every build
+	 *    with different commit hash.
+	 *  - Skip PCR[7] validation. PCR[7] contains UEFI Secure Boot variables
+	 *    measurement. These variables can not be updated through efi_selftest and
+	 *    it varies depending on the platform.
+	 */
+	for (i = 0; i < (EFI_TCG2_MAX_PCR_INDEX + 1); i++) {
+		result = pcrs + (TPM2_SHA256_DIGEST_SIZE * i);
+		if (i == 0 || i == 7) {
+			expected += TPM2_SHA256_DIGEST_SIZE;
+			result += TPM2_SHA256_DIGEST_SIZE;
+			continue; /* skip validation */
+		}
+		if (memcmp(expected, result, TPM2_SHA256_DIGEST_SIZE)) {
+			efi_st_printf("PCR[%d] is not the expected value\n", i);
+			return EFI_ST_FAILURE;
+		}
+		expected += TPM2_SHA256_DIGEST_SIZE;
+		result += TPM2_SHA256_DIGEST_SIZE;
+	}
 
 	return EFI_ST_SUCCESS;
 }
@@ -31,7 +680,8 @@ static int efi_st_tcg2_setup(const efi_handle_t img_handle,
 /**
  * efi_st_tcg2_execute() - execute test
  *
- * Call the GetCapability service of the EFI_TCG2_PROTOCOL.
+ * Call EFI_TCG2_PROTOCOL services and check the
+ * Measured Boot behavior.
  *
  * Return:	status code
  */
@@ -40,12 +690,22 @@ static int efi_st_tcg2_execute(void)
 	struct efi_tcg2_protocol *tcg2;
 	struct efi_tcg2_boot_service_capability capability;
 	efi_status_t ret;
+	u32 active_pcr_banks;
+	u64 eventlog, eventlog_last_entry;
+	bool eventlog_truncated;
+	efi_handle_t handle;
+	efi_uintn_t exit_data_size = 0;
+	u16 *exit_data = NULL;
+	u32 i;
+	u32 manufacturer_id;
 
 	ret = boottime->locate_protocol(&guid_tcg2, NULL, (void **)&tcg2);
 	if (ret != EFI_SUCCESS) {
 		efi_st_error("TCG2 protocol is not available.\n");
 		return EFI_ST_FAILURE;
 	}
+
+	/* EFI_TCG2_PROTOCOL.GetCapability test */
 	capability.size = sizeof(struct efi_tcg2_boot_service_capability) - 1;
 	ret = tcg2->get_capability(tcg2, &capability);
 	if (ret != EFI_BUFFER_TOO_SMALL) {
@@ -64,12 +724,154 @@ static int efi_st_tcg2_execute(void)
 	}
 	efi_st_printf("TPM supports 0x%.8x event logs\n",
 		      capability.supported_event_logs);
+
+	/* EFI_TCG2_PROTOCOL.GetActivePcrBanks test */
+	ret = tcg2->get_active_pcr_banks(tcg2, &active_pcr_banks);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("tcg2->get_active_pcr_banks failed\n");
+		return EFI_ST_FAILURE;
+	}
+	if (active_pcr_banks != capability.active_pcr_banks) {
+		efi_st_error("tcg2->get_active_pcr_banks return wrong value\n");
+		return EFI_ST_FAILURE;
+	}
+
+	/* EFI_TCG2_PROTOCOL.HashLogExtendEvent test */
+	ret = tcg2->hash_log_extend_event(tcg2, EFI_TCG2_EXTEND_ONLY,
+					  (uintptr_t)image,
+					  img.length, efi_tcg2_event);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("tcg2->hash_log_extend_event(EXTEND_ONLY) failed\n");
+		return EFI_ST_FAILURE;
+	}
+
+	ret = tcg2->hash_log_extend_event(tcg2, PE_COFF_IMAGE, (uintptr_t)image,
+					  img.length, efi_tcg2_event);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("tcg2->hash_log_extend_event(PE_COFF_IMAGE) failed\n");
+		return EFI_ST_FAILURE;
+	}
+
+	/* EFI_TCG2_PROTOCOL.SubmitCommand test */
+	ret = get_manufacturer_id(tcg2, &manufacturer_id);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("get_manufacturer_id failed\n");
+		return EFI_ST_FAILURE;
+	}
+	if (capability.manufacturer_id != manufacturer_id) {
+		efi_st_error("tcg2->submit_command test failed\n");
+		return EFI_ST_FAILURE;
+	}
+
+	/* tcg2_measure_pe_image test */
+	ret = boottime->load_image(false, image_handle, NULL, image,
+				   img.length, &handle);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Failed to load image\n");
+		return EFI_ST_FAILURE;
+	}
+
+	/* measure ready_to_boot event(boot variables, smbios table, etc.) */
+	/* TODO: add GPT measurement test */
+	ret = boottime->start_image(handle, &exit_data_size, &exit_data);
+	if (ret != EFI_UNSUPPORTED) {
+		efi_st_error("Wrong return value from application\n");
+		return EFI_ST_FAILURE;
+	}
+	ret = boottime->free_pool(exit_data);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Failed to free exit data\n");
+		return EFI_ST_FAILURE;
+	}
+
+	/* validate PCR read from the TPM device */
+	for (i = 0; i < (EFI_TCG2_MAX_PCR_INDEX + 1); i++) {
+		ret = read_pcr(tcg2, i);
+		if (ret != EFI_SUCCESS) {
+			efi_st_error("read pcr error\n");
+			return EFI_ST_FAILURE;
+		}
+	}
+	if (validate_pcrs()) {
+		efi_st_error("PCR validation failed\n");
+		return EFI_ST_FAILURE;
+	}
+
+	/* EFI_TCG2_PROTOCOL.GetEventLog test */
+	ret = tcg2->get_eventlog(tcg2, TCG2_EVENT_LOG_FORMAT_TCG_2, &eventlog,
+				 &eventlog_last_entry, &eventlog_truncated);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("tcg2->get_eventlog failed\n");
+		return EFI_ST_FAILURE;
+	}
+	/* TODO: eventlog format check */
+
 	return EFI_ST_SUCCESS;
 }
 
+/*
+ * Tear down unit test.
+ *
+ * @return:	EFI_ST_SUCCESS for success
+ */
+static int efi_st_tcg2_teardown(void)
+{
+	efi_status_t r = EFI_ST_SUCCESS;
+
+	if (image) {
+		r = boottime->free_pool(image);
+		if (r != EFI_SUCCESS) {
+			efi_st_error("Failed to free image\n");
+			return EFI_ST_FAILURE;
+		}
+	}
+	if (efi_tcg2_event) {
+		r = boottime->free_pool(efi_tcg2_event);
+		if (r != EFI_SUCCESS) {
+			efi_st_error("Failed to free efi_tcg2_event\n");
+			return EFI_ST_FAILURE;
+		}
+	}
+	if (pcrs) {
+		r = boottime->free_pool(pcrs);
+		if (r != EFI_SUCCESS) {
+			efi_st_error("Failed to free pcr\n");
+			return EFI_ST_FAILURE;
+		}
+	}
+
+	r = restore_boot_variable();
+	if (r != EFI_SUCCESS) {
+		efi_st_error("Failed to restore boot variables\n");
+		return EFI_ST_FAILURE;
+	}
+
+	/*
+	 * Restore SMBIOS table
+	 * If orig_smbios_table is NULL, calling install_configuration_table()
+	 * removes dummy SMBIOS table form systab.
+	 */
+	r = boottime->install_configuration_table(&smbios_guid, orig_smbios_table);
+	if (r != EFI_SUCCESS) {
+		efi_st_error("Failed to restore SMBOIS table\n");
+		return EFI_ST_FAILURE;
+	}
+
+	if (dmi_addr) {
+		r = boottime->free_pages(dmi_addr, 1);
+		if (r != EFI_SUCCESS) {
+			efi_st_error("Failed to free dummy smbios table\n");
+			return EFI_ST_FAILURE;
+		}
+	}
+
+	return r;
+}
+
 EFI_UNIT_TEST(tcg2) = {
 	.name = "tcg2",
 	.phase = EFI_EXECUTE_BEFORE_BOOTTIME_EXIT,
 	.execute = efi_st_tcg2_execute,
 	.setup = efi_st_tcg2_setup,
+	.teardown = efi_st_tcg2_teardown,
 };
