Merge tag 'signed-efi-next' of git://github.com/agraf/u-boot

Patch queue for efi - 2018-01-23

This time around we have a lot of EFI patches from Heinrich.
Highlights are:

  - Allow EFI applications to register as drivers
  - Allow exposure of U-Boot block devices from an EFI payload
  - Compatibility improvements

44 files changed, 3508 insertions, 345 deletions

diff --git a/MAINTAINERS b/MAINTAINERS
index 754db5553d..d459153503 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -287,9 +287,10 @@ M:	Alexander Graf <agraf@suse.de>
 S:	Maintained
 T:	git git://github.com/agraf/u-boot.git
 F:	include/efi*
-F:	lib/efi*
+F:	lib/efi*/
 F:	test/py/tests/test_efi*
 F:	cmd/bootefi.c
+F:	tools/file2include.c
 
 FLATTENED DEVICE TREE
 M:	Simon Glass <sjg@chromium.org>
diff --git a/arch/arm/lib/Makefile b/arch/arm/lib/Makefile
index abffa10c85..876024fc15 100644
--- a/arch/arm/lib/Makefile
+++ b/arch/arm/lib/Makefile
@@ -112,4 +112,5 @@ CFLAGS_$(EFI_RELOC) := $(CFLAGS_EFI)
 CFLAGS_REMOVE_$(EFI_RELOC) := $(CFLAGS_NON_EFI)
 
 extra-$(CONFIG_CMD_BOOTEFI_HELLO_COMPILE) += $(EFI_CRT0) $(EFI_RELOC)
+extra-$(CONFIG_CMD_BOOTEFI_SELFTEST) += $(EFI_CRT0) $(EFI_RELOC)
 extra-$(CONFIG_EFI) += $(EFI_CRT0) $(EFI_RELOC)
diff --git a/cmd/bootefi.c b/cmd/bootefi.c
index 78ff109835..51213c0293 100644
--- a/cmd/bootefi.c
+++ b/cmd/bootefi.c
@@ -32,6 +32,9 @@ static void efi_init_obj_list(void)
 {
 	efi_obj_list_initalized = 1;
 
+	/* Initialize EFI driver uclass */
+	efi_driver_init();
+
 	efi_console_register();
 #ifdef CONFIG_PARTITIONS
 	efi_disk_register();
@@ -103,11 +106,11 @@ static void *copy_fdt(void *fdt)
 
 	/* Safe fdt location is at 128MB */
 	new_fdt_addr = fdt_ram_start + (128 * 1024 * 1024) + fdt_size;
-	if (efi_allocate_pages(1, EFI_BOOT_SERVICES_DATA, fdt_pages,
+	if (efi_allocate_pages(1, EFI_RUNTIME_SERVICES_DATA, fdt_pages,
 			       &new_fdt_addr) != EFI_SUCCESS) {
 		/* If we can't put it there, put it somewhere */
 		new_fdt_addr = (ulong)memalign(EFI_PAGE_SIZE, fdt_size);
-		if (efi_allocate_pages(1, EFI_BOOT_SERVICES_DATA, fdt_pages,
+		if (efi_allocate_pages(1, EFI_RUNTIME_SERVICES_DATA, fdt_pages,
 				       &new_fdt_addr) != EFI_SUCCESS) {
 			printf("ERROR: Failed to reserve space for FDT\n");
 			return NULL;
@@ -122,8 +125,8 @@ static void *copy_fdt(void *fdt)
 }
 
 static efi_status_t efi_do_enter(
-			void *image_handle, struct efi_system_table *st,
-			asmlinkage ulong (*entry)(void *image_handle,
+			efi_handle_t image_handle, struct efi_system_table *st,
+			asmlinkage ulong (*entry)(efi_handle_t image_handle,
 						  struct efi_system_table *st))
 {
 	efi_status_t ret = EFI_LOAD_ERROR;
@@ -136,8 +139,8 @@ static efi_status_t efi_do_enter(
 
 #ifdef CONFIG_ARM64
 static efi_status_t efi_run_in_el2(asmlinkage ulong (*entry)(
-			void *image_handle, struct efi_system_table *st),
-			void *image_handle, struct efi_system_table *st)
+			efi_handle_t image_handle, struct efi_system_table *st),
+			efi_handle_t image_handle, struct efi_system_table *st)
 {
 	/* Enable caches again */
 	dcache_enable();
@@ -159,7 +162,7 @@ static efi_status_t do_bootefi_exec(void *efi, void *fdt,
 	struct efi_device_path *memdp = NULL;
 	ulong ret;
 
-	ulong (*entry)(void *image_handle, struct efi_system_table *st)
+	ulong (*entry)(efi_handle_t image_handle, struct efi_system_table *st)
 		asmlinkage;
 	ulong fdt_pages, fdt_size, fdt_start, fdt_end;
 	const efi_guid_t fdt_guid = EFI_FDT_GUID;
diff --git a/drivers/block/blk-uclass.c b/drivers/block/blk-uclass.c
index 010ed32d3a..bfda2211f0 100644
--- a/drivers/block/blk-uclass.c
+++ b/drivers/block/blk-uclass.c
@@ -24,6 +24,7 @@ static const char *if_typename_str[IF_TYPE_COUNT] = {
 	[IF_TYPE_HOST]		= "host",
 	[IF_TYPE_SYSTEMACE]	= "ace",
 	[IF_TYPE_NVME]		= "nvme",
+	[IF_TYPE_EFI]		= "efi",
 };
 
 static enum uclass_id if_type_uclass_id[IF_TYPE_COUNT] = {
@@ -36,8 +37,9 @@ static enum uclass_id if_type_uclass_id[IF_TYPE_COUNT] = {
 	[IF_TYPE_SD]		= UCLASS_INVALID,
 	[IF_TYPE_SATA]		= UCLASS_AHCI,
 	[IF_TYPE_HOST]		= UCLASS_ROOT,
-	[IF_TYPE_NVME]		= UCLASS_NVME,
 	[IF_TYPE_SYSTEMACE]	= UCLASS_INVALID,
+	[IF_TYPE_NVME]		= UCLASS_NVME,
+	[IF_TYPE_EFI]		= UCLASS_EFI,
 };
 
 static enum if_type if_typename_to_iftype(const char *if_typename)
diff --git a/examples/api/Makefile b/examples/api/Makefile
index 899527267d..9068727b98 100644
--- a/examples/api/Makefile
+++ b/examples/api/Makefile
@@ -4,6 +4,9 @@
 # SPDX-License-Identifier:	GPL-2.0+
 #
 
+# Provide symbol API_BUILD to signal that the API example is being built.
+KBUILD_CPPFLAGS += -DAPI_BUILD
+
 ifeq ($(ARCH),powerpc)
 LOAD_ADDR = 0x40000
 endif
diff --git a/include/blk.h b/include/blk.h
index 41b4d7efa8..69b5a98e56 100644
--- a/include/blk.h
+++ b/include/blk.h
@@ -34,6 +34,7 @@ enum if_type {
 	IF_TYPE_HOST,
 	IF_TYPE_SYSTEMACE,
 	IF_TYPE_NVME,
+	IF_TYPE_EFI,
 
 	IF_TYPE_COUNT,			/* Number of interface types */
 };
diff --git a/include/config_fallbacks.h b/include/config_fallbacks.h
index 76b13c3b30..9695ee7ffb 100644
--- a/include/config_fallbacks.h
+++ b/include/config_fallbacks.h
@@ -39,6 +39,7 @@
 	defined(CONFIG_MMC) || \
 	defined(CONFIG_NVME) || \
 	defined(CONFIG_SYSTEMACE) || \
+	(defined(CONFIG_EFI_LOADER) && !defined(CONFIG_SPL_BUILD)) || \
 	defined(CONFIG_SANDBOX)
 #define HAVE_BLOCK_DEVICE
 #endif
diff --git a/include/dm/uclass-id.h b/include/dm/uclass-id.h
index 3fc20834ae..07fabc3ce6 100644
--- a/include/dm/uclass-id.h
+++ b/include/dm/uclass-id.h
@@ -34,6 +34,7 @@ enum uclass_id {
 	UCLASS_CROS_EC,		/* Chrome OS EC */
 	UCLASS_DISPLAY,		/* Display (e.g. DisplayPort, HDMI) */
 	UCLASS_DMA,		/* Direct Memory Access */
+	UCLASS_EFI,		/* EFI managed devices */
 	UCLASS_ETH,		/* Ethernet device */
 	UCLASS_GPIO,		/* Bank of general-purpose I/O pins */
 	UCLASS_FIRMWARE,	/* Firmware */
diff --git a/include/efi_api.h b/include/efi_api.h
index 584016dc30..205f8f1f70 100644
--- a/include/efi_api.h
+++ b/include/efi_api.h
@@ -84,11 +84,12 @@ struct efi_boot_services {
 	efi_status_t (EFIAPI *reinstall_protocol_interface)(
 			void *handle, const efi_guid_t *protocol,
 			void *old_interface, void *new_interface);
-	efi_status_t (EFIAPI *uninstall_protocol_interface)(void *handle,
-			const efi_guid_t *protocol, void *protocol_interface);
-	efi_status_t (EFIAPI *handle_protocol)(efi_handle_t,
-					       const efi_guid_t *protocol,
-					       void **protocol_interface);
+	efi_status_t (EFIAPI *uninstall_protocol_interface)(
+			efi_handle_t handle, const efi_guid_t *protocol,
+			void *protocol_interface);
+	efi_status_t (EFIAPI *handle_protocol)(
+			efi_handle_t handle, const efi_guid_t *protocol,
+			void **protocol_interface);
 	void *reserved;
 	efi_status_t (EFIAPI *register_protocol_notify)(
 			const efi_guid_t *protocol, struct efi_event *event,
@@ -113,7 +114,7 @@ struct efi_boot_services {
 	efi_status_t (EFIAPI *exit)(efi_handle_t handle,
 				    efi_status_t exit_status,
 				    unsigned long exitdata_size, s16 *exitdata);
-	efi_status_t (EFIAPI *unload_image)(void *image_handle);
+	efi_status_t (EFIAPI *unload_image)(efi_handle_t image_handle);
 	efi_status_t (EFIAPI *exit_boot_services)(efi_handle_t, unsigned long);
 
 	efi_status_t (EFIAPI *get_next_monotonic_count)(u64 *count);
@@ -125,8 +126,10 @@ struct efi_boot_services {
 			efi_handle_t *driver_image_handle,
 			struct efi_device_path *remaining_device_path,
 			bool recursive);
-	efi_status_t (EFIAPI *disconnect_controller)(void *controller_handle,
-			void *driver_image_handle, void *child_handle);
+	efi_status_t (EFIAPI *disconnect_controller)(
+			efi_handle_t controller_handle,
+			efi_handle_t driver_image_handle,
+			efi_handle_t child_handle);
 #define EFI_OPEN_PROTOCOL_BY_HANDLE_PROTOCOL  0x00000001
 #define EFI_OPEN_PROTOCOL_GET_PROTOCOL        0x00000002
 #define EFI_OPEN_PROTOCOL_TEST_PROTOCOL       0x00000004
@@ -137,9 +140,10 @@ struct efi_boot_services {
 			const efi_guid_t *protocol, void **interface,
 			efi_handle_t agent_handle,
 			efi_handle_t controller_handle, u32 attributes);
-	efi_status_t (EFIAPI *close_protocol)(void *handle,
-			const efi_guid_t *protocol, void *agent_handle,
-			void *controller_handle);
+	efi_status_t (EFIAPI *close_protocol)(
+			efi_handle_t handle, const efi_guid_t *protocol,
+			efi_handle_t agent_handle,
+			efi_handle_t controller_handle);
 	efi_status_t(EFIAPI *open_protocol_information)(efi_handle_t handle,
 			const efi_guid_t *protocol,
 			struct efi_open_protocol_info_entry **entry_buffer,
@@ -243,11 +247,11 @@ struct efi_system_table {
 	struct efi_table_hdr hdr;
 	unsigned long fw_vendor;   /* physical addr of wchar_t vendor string */
 	u32 fw_revision;
-	unsigned long con_in_handle;
+	efi_handle_t con_in_handle;
 	struct efi_simple_input_interface *con_in;
-	unsigned long con_out_handle;
+	efi_handle_t con_out_handle;
 	struct efi_simple_text_output_protocol *con_out;
-	unsigned long stderr_handle;
+	efi_handle_t stderr_handle;
 	struct efi_simple_text_output_protocol *std_err;
 	struct efi_runtime_services *runtime;
 	struct efi_boot_services *boottime;
@@ -329,12 +333,27 @@ struct efi_device_path_acpi_path {
 } __packed;
 
 #define DEVICE_PATH_TYPE_MESSAGING_DEVICE	0x03
+#  define DEVICE_PATH_SUB_TYPE_MSG_ATAPI	0x01
+#  define DEVICE_PATH_SUB_TYPE_MSG_SCSI		0x02
 #  define DEVICE_PATH_SUB_TYPE_MSG_USB		0x05
 #  define DEVICE_PATH_SUB_TYPE_MSG_MAC_ADDR	0x0b
 #  define DEVICE_PATH_SUB_TYPE_MSG_USB_CLASS	0x0f
 #  define DEVICE_PATH_SUB_TYPE_MSG_SD		0x1a
 #  define DEVICE_PATH_SUB_TYPE_MSG_MMC		0x1d
 
+struct efi_device_path_atapi {
+	struct efi_device_path dp;
+	u8 primary_secondary;
+	u8 slave_master;
+	u16 logical_unit_number;
+} __packed;
+
+struct efi_device_path_scsi {
+	struct efi_device_path dp;
+	u16 target_id;
+	u16 logical_unit_number;
+} __packed;
+
 struct efi_device_path_usb {
 	struct efi_device_path dp;
 	u8 parent_port_number;
@@ -405,18 +424,26 @@ struct efi_block_io_media
 	u32 io_align;
 	u8 pad2[4];
 	u64 last_block;
+	/* Added in revision 2 of the protocol */
+	u64 lowest_aligned_lba;
+	u32 logical_blocks_per_physical_block;
+	/* Added in revision 3 of the protocol */
+	u32 optimal_transfer_length_granualarity;
 };
 
+#define EFI_BLOCK_IO_PROTOCOL_REVISION2	0x00020001
+#define EFI_BLOCK_IO_PROTOCOL_REVISION3	0x0002001f
+
 struct efi_block_io {
 	u64 revision;
 	struct efi_block_io_media *media;
 	efi_status_t (EFIAPI *reset)(struct efi_block_io *this,
 			char extended_verification);
 	efi_status_t (EFIAPI *read_blocks)(struct efi_block_io *this,
-			u32 media_id, u64 lba, unsigned long buffer_size,
+			u32 media_id, u64 lba, efi_uintn_t buffer_size,
 			void *buffer);
 	efi_status_t (EFIAPI *write_blocks)(struct efi_block_io *this,
-			u32 media_id, u64 lba, unsigned long buffer_size,
+			u32 media_id, u64 lba, efi_uintn_t buffer_size,
 			void *buffer);
 	efi_status_t (EFIAPI *flush_blocks)(struct efi_block_io *this);
 };
@@ -790,4 +817,26 @@ struct efi_file_info {
 	s16 file_name[0];
 };
 
+#define EFI_DRIVER_BINDING_PROTOCOL_GUID \
+	EFI_GUID(0x18a031ab, 0xb443, 0x4d1a,\
+		 0xa5, 0xc0, 0x0c, 0x09, 0x26, 0x1e, 0x9f, 0x71)
+struct efi_driver_binding_protocol {
+	efi_status_t (EFIAPI * supported)(
+			struct efi_driver_binding_protocol *this,
+			efi_handle_t controller_handle,
+			struct efi_device_path *remaining_device_path);
+	efi_status_t (EFIAPI * start)(
+			struct efi_driver_binding_protocol *this,
+			efi_handle_t controller_handle,
+			struct efi_device_path *remaining_device_path);
+	efi_status_t (EFIAPI * stop)(
+			struct efi_driver_binding_protocol *this,
+			efi_handle_t controller_handle,
+			efi_uintn_t number_of_children,
+			efi_handle_t *child_handle_buffer);
+	u32 version;
+	efi_handle_t image_handle;
+	efi_handle_t driver_binding_handle;
+};
+
 #endif
diff --git a/include/efi_driver.h b/include/efi_driver.h
new file mode 100644
index 0000000000..2bbe26c6e3
--- /dev/null
+++ b/include/efi_driver.h
@@ -0,0 +1,30 @@
+/*
+ *  EFI application loader
+ *
+ *  Copyright (c) 2017 Heinrich Schuchardt
+ *
+ *  SPDX-License-Identifier:     GPL-2.0+
+ */
+
+#ifndef _EFI_DRIVER_H
+#define _EFI_DRIVER_H 1
+
+#include <common.h>
+#include <dm.h>
+#include <efi_loader.h>
+
+struct efi_driver_ops {
+	const efi_guid_t *protocol;
+	const efi_guid_t *child_protocol;
+	int (*bind)(efi_handle_t handle, void *interface);
+};
+
+/*
+ * This structure adds internal fields to the driver binding protocol.
+ */
+struct efi_driver_binding_extended_protocol {
+	struct efi_driver_binding_protocol bp;
+	const struct efi_driver_ops *ops;
+};
+
+#endif /* _EFI_DRIVER_H */
diff --git a/include/efi_loader.h b/include/efi_loader.h
index 6185055e78..21c03c5c28 100644
--- a/include/efi_loader.h
+++ b/include/efi_loader.h
@@ -69,10 +69,11 @@ const char *__efi_nesting_dec(void);
 	} while(0)
 
 /*
- * Write GUID
+ * Write an indented message with EFI prefix
  */
-#define EFI_PRINT_GUID(txt, guid) ({ \
-	debug("%sEFI: %s %pUl\n", __efi_nesting(), txt, guid); \
+#define EFI_PRINT(format, ...) ({ \
+	debug("%sEFI: " format, __efi_nesting(), \
+		##__VA_ARGS__); \
 	})
 
 extern struct efi_runtime_services efi_runtime_services;
@@ -85,9 +86,13 @@ extern const struct efi_device_path_to_text_protocol efi_device_path_to_text;
 
 uint16_t *efi_dp_str(struct efi_device_path *dp);
 
+/* GUID of the EFI_BLOCK_IO_PROTOCOL */
+extern const efi_guid_t efi_block_io_guid;
 extern const efi_guid_t efi_global_variable_guid;
 extern const efi_guid_t efi_guid_console_control;
 extern const efi_guid_t efi_guid_device_path;
+/* GUID of the EFI_DRIVER_BINDING_PROTOCOL */
+extern const efi_guid_t efi_guid_driver_binding_protocol;
 extern const efi_guid_t efi_guid_loaded_image;
 extern const efi_guid_t efi_guid_device_path_to_text_protocol;
 extern const efi_guid_t efi_simple_file_system_protocol_guid;
@@ -97,14 +102,27 @@ extern unsigned int __efi_runtime_start, __efi_runtime_stop;
 extern unsigned int __efi_runtime_rel_start, __efi_runtime_rel_stop;
 
 /*
+ * When a protocol is opened a open protocol info entry is created.
+ * These are maintained in a list.
+ */
+struct efi_open_protocol_info_item {
+	/* Link to the list of open protocol info entries of a protocol */
+	struct list_head link;
+	struct efi_open_protocol_info_entry info;
+};
+
+/*
  * When the UEFI payload wants to open a protocol on an object to get its
  * interface (usually a struct with callback functions), this struct maps the
- * protocol GUID to the respective protocol interface */
+ * protocol GUID to the respective protocol interface
+ */
 struct efi_handler {
 	/* Link to the list of protocols of a handle */
 	struct list_head link;
 	const efi_guid_t *guid;
 	void *protocol_interface;
+	/* Link to the list of open protocol info items */
+	struct list_head open_infos;
 };
 
 /*
@@ -156,6 +174,10 @@ extern struct list_head efi_obj_list;
 int efi_console_register(void);
 /* Called by bootefi to make all disk storage accessible as EFI objects */
 int efi_disk_register(void);
+/* Create handles and protocols for the partitions of a block device */
+int efi_disk_create_partitions(efi_handle_t parent, struct blk_desc *desc,
+			       const char *if_typename, int diskid,
+			       const char *pdevname);
 /* Called by bootefi to make GOP (graphical) interface available */
 int efi_gop_register(void);
 /* Called by bootefi to make the network interface available */
@@ -189,23 +211,25 @@ void efi_set_bootdev(const char *dev, const char *devnr, const char *path);
 /* Add a new object to the object list. */
 void efi_add_handle(struct efi_object *obj);
 /* Create handle */
-efi_status_t efi_create_handle(void **handle);
+efi_status_t efi_create_handle(efi_handle_t *handle);
 /* Delete handle */
 void efi_delete_handle(struct efi_object *obj);
 /* Call this to validate a handle and find the EFI object for it */
-struct efi_object *efi_search_obj(const void *handle);
+struct efi_object *efi_search_obj(const efi_handle_t handle);
 /* Find a protocol on a handle */
-efi_status_t efi_search_protocol(const void *handle,
+efi_status_t efi_search_protocol(const efi_handle_t handle,
 				 const efi_guid_t *protocol_guid,
 				 struct efi_handler **handler);
 /* Install new protocol on a handle */
-efi_status_t efi_add_protocol(const void *handle, const efi_guid_t *protocol,
+efi_status_t efi_add_protocol(const efi_handle_t handle,
+			      const efi_guid_t *protocol,
 			      void *protocol_interface);
 /* Delete protocol from a handle */
-efi_status_t efi_remove_protocol(const void *handle, const efi_guid_t *protocol,
+efi_status_t efi_remove_protocol(const efi_handle_t handle,
+				 const efi_guid_t *protocol,
 				 void *protocol_interface);
 /* Delete all protocols from a handle */
-efi_status_t efi_remove_all_protocols(const void *handle);
+efi_status_t efi_remove_all_protocols(const efi_handle_t handle);
 /* Call this to create an event */
 efi_status_t efi_create_event(uint32_t type, efi_uintn_t notify_tpl,
 			      void (EFIAPI *notify_function) (
@@ -216,7 +240,7 @@ efi_status_t efi_create_event(uint32_t type, efi_uintn_t notify_tpl,
 efi_status_t efi_set_timer(struct efi_event *event, enum efi_timer_delay type,
 			   uint64_t trigger_time);
 /* Call this to signal an event */
-void efi_signal_event(struct efi_event *event);
+void efi_signal_event(struct efi_event *event, bool check_tpl);
 
 /* open file system: */
 struct efi_simple_file_system_protocol *efi_simple_file_system(
@@ -247,6 +271,8 @@ efi_status_t efi_get_memory_map(efi_uintn_t *memory_map_size,
 /* Adds a range into the EFI memory map */
 uint64_t efi_add_memory_map(uint64_t start, uint64_t pages, int memory_type,
 			    bool overlap_only_ram);
+/* Called by board init to initialize the EFI drivers */
+int efi_driver_init(void);
 /* Called by board init to initialize the EFI memory map */
 int efi_memory_init(void);
 /* Adds new or overrides configuration table entry to the system table */
@@ -280,15 +306,20 @@ struct efi_device_path *efi_dp_append_node(const struct efi_device_path *dp,
 
 struct efi_device_path *efi_dp_from_dev(struct udevice *dev);
 struct efi_device_path *efi_dp_from_part(struct blk_desc *desc, int part);
+/* Create a device node for a block device partition. */
+struct efi_device_path *efi_dp_part_node(struct blk_desc *desc, int part);
 struct efi_device_path *efi_dp_from_file(struct blk_desc *desc, int part,
 					 const char *path);
 struct efi_device_path *efi_dp_from_eth(void);
 struct efi_device_path *efi_dp_from_mem(uint32_t mem_type,
 					uint64_t start_address,
 					uint64_t end_address);
-void efi_dp_split_file_path(struct efi_device_path *full_path,
-			    struct efi_device_path **device_path,
-			    struct efi_device_path **file_path);
+/* Determine the last device path node that is not the end node. */
+const struct efi_device_path *efi_dp_last_node(
+			const struct efi_device_path *dp);
+efi_status_t efi_dp_split_file_path(struct efi_device_path *full_path,
+				    struct efi_device_path **device_path,
+				    struct efi_device_path **file_path);
 
 #define EFI_DP_TYPE(_dp, _type, _subtype) \
 	(((_dp)->type == DEVICE_PATH_TYPE_##_type) && \
diff --git a/include/efi_selftest.h b/include/efi_selftest.h
index be5ba4bfa9..08dd8e43ad 100644
--- a/include/efi_selftest.h
+++ b/include/efi_selftest.h
@@ -19,13 +19,19 @@
 #define EFI_ST_FAILURE 1
 
 /*
+ * Prints a message.
+ */
+#define efi_st_printf(...) \
+	(efi_st_printc(-1, __VA_ARGS__))
+
+/*
  * Prints an error message.
  *
  * @...	format string followed by fields to print
  */
 #define efi_st_error(...) \
-	(efi_st_printf("%s(%u):\nERROR: ", __FILE__, __LINE__), \
-	efi_st_printf(__VA_ARGS__)) \
+	(efi_st_printc(EFI_LIGHTRED, "%s(%u):\nERROR: ", __FILE__, __LINE__), \
+	efi_st_printc(EFI_LIGHTRED, __VA_ARGS__))
 
 /*
  * Prints a TODO message.
@@ -33,8 +39,8 @@
  * @...	format string followed by fields to print
  */
 #define efi_st_todo(...) \
-	(efi_st_printf("%s(%u):\nTODO: ", __FILE__, __LINE__), \
-	efi_st_printf(__VA_ARGS__)) \
+	(efi_st_printc(EFI_YELLOW, "%s(%u):\nTODO: ", __FILE__, __LINE__), \
+	efi_st_printc(EFI_YELLOW, __VA_ARGS__)) \
 
 /*
  * A test may be setup and executed at boottime,
@@ -61,14 +67,15 @@ extern struct efi_simple_input_interface *con_in;
 void efi_st_exit_boot_services(void);
 
 /*
- * Print a pointer to an u16 string
+ * Print a colored message
  *
- * @pointer: pointer
- * @buf: pointer to buffer address
- * on return position of terminating zero word
+ * @color	color, see constants in efi_api.h, use -1 for no color
+ * @fmt		printf format
+ * @...		arguments to be printed
+ *		on return position of terminating zero word
  */
-void efi_st_printf(const char *fmt, ...)
-		 __attribute__ ((format (__printf__, 1, 2)));
+void efi_st_printc(int color, const char *fmt, ...)
+		 __attribute__ ((format (__printf__, 2, 3)));
 
 /*
  * Compare memory.
diff --git a/lib/Makefile b/lib/Makefile
index 8cd779f8ca..0db41c19f3 100644
--- a/lib/Makefile
+++ b/lib/Makefile
@@ -8,6 +8,7 @@
 ifndef CONFIG_SPL_BUILD
 
 obj-$(CONFIG_EFI) += efi/
+obj-$(CONFIG_EFI_LOADER) += efi_driver/
 obj-$(CONFIG_EFI_LOADER) += efi_loader/
 obj-$(CONFIG_EFI_LOADER) += efi_selftest/
 obj-$(CONFIG_LZMA) += lzma/
diff --git a/lib/efi_driver/Makefile b/lib/efi_driver/Makefile
new file mode 100644
index 0000000000..e35529a952
--- /dev/null
+++ b/lib/efi_driver/Makefile
@@ -0,0 +1,13 @@
+#
+# (C) Copyright 2017 Heinrich Schuchardt
+#
+#  SPDX-License-Identifier:     GPL-2.0+
+#
+
+# This file only gets included with CONFIG_EFI_LOADER set, so all
+# object inclusion implicitly depends on it
+
+obj-y += efi_uclass.o
+ifeq ($(CONFIG_BLK)$(CONFIG_PARTITIONS),yy)
+obj-y += efi_block_device.o
+endif
diff --git a/lib/efi_driver/efi_block_device.c b/lib/efi_driver/efi_block_device.c
new file mode 100644
index 0000000000..d9d2b14f61
--- /dev/null
+++ b/lib/efi_driver/efi_block_device.c
@@ -0,0 +1,210 @@
+/*
+ *  EFI block driver
+ *
+ *  Copyright (c) 2017 Heinrich Schuchardt
+ *
+ *  SPDX-License-Identifier:     GPL-2.0+
+ *
+ * The EFI uclass creates a handle for this driver and installs the
+ * driver binding protocol on it.
+ *
+ * The EFI block driver binds to controllers implementing the block io
+ * protocol.
+ *
+ * When the bind function of the EFI block driver is called it creates a
+ * new U-Boot block device. It installs child handles for all partitions and
+ * installs the simple file protocol on these.
+ *
+ * The read and write functions of the EFI block driver delegate calls to the
+ * controller that it is bound to.
+ *
+ * A usage example is as following:
+ *
+ * U-Boot loads the iPXE snp.efi executable. iPXE connects an iSCSI drive and
+ * exposes a handle with the block IO protocol. It calls ConnectController.
+ *
+ * Now the EFI block driver installs the partitions with the simple file
+ * protocol.
+ *
+ * iPXE uses the simple file protocol to load Grub or the Linux Kernel.
+ */
+
+#include <efi_driver.h>
+#include <dm/device-internal.h>
+#include <dm/root.h>
+
+/*
+ * EFI attributes of the udevice handled by this driver.
+ *
+ * handle	handle of the controller on which this driver is installed
+ * io		block io protocol proxied by this driver
+ */
+struct efi_blk_priv {
+	efi_handle_t		handle;
+	struct efi_block_io	*io;
+};
+
+/*
+ * Read from block device
+ *
+ * @dev		device
+ * @blknr	first block to be read
+ * @blkcnt	number of blocks to read
+ * @buffer	output buffer
+ * @return	number of blocks transferred
+ */
+static ulong efi_bl_read(struct udevice *dev, lbaint_t blknr, lbaint_t blkcnt,
+			 void *buffer)
+{
+	struct efi_blk_priv *priv = dev->priv;
+	struct efi_block_io *io = priv->io;
+	efi_status_t ret;
+
+	EFI_PRINT("%s: read '%s', from block " LBAFU ", " LBAFU " blocks\n",
+		  __func__, dev->name, blknr, blkcnt);
+	ret = EFI_CALL(io->read_blocks(
+				io, io->media->media_id, (u64)blknr,
+				(efi_uintn_t)blkcnt *
+				(efi_uintn_t)io->media->block_size, buffer));
+	EFI_PRINT("%s: r = %u\n", __func__,
+		  (unsigned int)(ret & ~EFI_ERROR_MASK));
+	if (ret != EFI_SUCCESS)
+		return 0;
+	return blkcnt;
+}
+
+/*
+ * Write to block device
+ *
+ * @dev		device
+ * @blknr	first block to be write
+ * @blkcnt	number of blocks to write
+ * @buffer	input buffer
+ * @return	number of blocks transferred
+ */
+static ulong efi_bl_write(struct udevice *dev, lbaint_t blknr, lbaint_t blkcnt,
+			  const void *buffer)
+{
+	struct efi_blk_priv *priv = dev->priv;
+	struct efi_block_io *io = priv->io;
+	efi_status_t ret;
+
+	EFI_PRINT("%s: write '%s', from block " LBAFU ", " LBAFU " blocks\n",
+		  __func__, dev->name, blknr, blkcnt);
+	ret = EFI_CALL(io->write_blocks(
+				io, io->media->media_id, (u64)blknr,
+				(efi_uintn_t)blkcnt *
+				(efi_uintn_t)io->media->block_size,
+				(void *)buffer));
+	EFI_PRINT("%s: r = %u\n", __func__,
+		  (unsigned int)(ret & ~EFI_ERROR_MASK));
+	if (ret != EFI_SUCCESS)
+		return 0;
+	return blkcnt;
+}
+
+/*
+ * Create partions for the block device.
+ *
+ * @handle	EFI handle of the block device
+ * @dev		udevice of the block device
+ */
+static int efi_bl_bind_partitions(efi_handle_t handle, struct udevice *dev)
+{
+	struct blk_desc *desc;
+	const char *if_typename;
+
+	desc = dev_get_uclass_platdata(dev);
+	if_typename = blk_get_if_type_name(desc->if_type);
+
+	return efi_disk_create_partitions(handle, desc, if_typename,
+					  desc->devnum, dev->name);
+}
+
+/*
+ * Create a block device for a handle
+ *
+ * @handle	handle
+ * @interface	block io protocol
+ * @return	0 = success
+ */
+static int efi_bl_bind(efi_handle_t handle, void *interface)
+{
+	struct udevice *bdev, *parent = dm_root();
+	int ret, devnum;
+	char *name;
+	struct efi_object *obj = efi_search_obj(handle);
+	struct efi_block_io *io = interface;
+	int disks;
+	struct efi_blk_priv *priv;
+
+	EFI_PRINT("%s: handle %p, interface %p\n", __func__, handle, io);
+
+	if (!obj)
+		return -ENOENT;
+
+	devnum = blk_find_max_devnum(IF_TYPE_EFI);
+	if (devnum == -ENODEV)
+		devnum = 0;
+	else if (devnum < 0)
+		return devnum;
+
+	name = calloc(1, 18); /* strlen("efiblk#2147483648") + 1 */
+	if (!name)
+		return -ENOMEM;
+	sprintf(name, "efiblk#%d", devnum);
+
+	/* Create driver model udevice for the EFI block io device */
+	ret = blk_create_device(parent, "efi_blk", name, IF_TYPE_EFI, devnum,
+				io->media->block_size,
+				(lbaint_t)io->media->last_block, &bdev);
+	if (ret)
+		return ret;
+	if (!bdev)
+		return -ENOENT;
+	/* Allocate priv */
+	ret = device_probe(bdev);
+	if (ret)
+		return ret;
+	EFI_PRINT("%s: block device '%s' created\n", __func__, bdev->name);
+
+	priv = bdev->priv;
+	priv->handle = handle;
+	priv->io = interface;
+
+	ret = blk_prepare_device(bdev);
+
+	/* Create handles for the partions of the block device */
+	disks = efi_bl_bind_partitions(handle, bdev);
+	EFI_PRINT("Found %d partitions\n", disks);
+
+	return 0;
+}
+
+/* Block device driver operators */
+static const struct blk_ops efi_blk_ops = {
+	.read	= efi_bl_read,
+	.write	= efi_bl_write,
+};
+
+/* Identify as block device driver */
+U_BOOT_DRIVER(efi_blk) = {
+	.name			= "efi_blk",
+	.id			= UCLASS_BLK,
+	.ops			= &efi_blk_ops,
+	.priv_auto_alloc_size	= sizeof(struct efi_blk_priv),
+};
+
+/* EFI driver operators */
+static const struct efi_driver_ops driver_ops = {
+	.protocol	= &efi_block_io_guid,
+	.child_protocol = &efi_block_io_guid,
+	.bind		= efi_bl_bind,
+};
+
+/* Identify as EFI driver */
+U_BOOT_DRIVER(efi_block) = {
+	.name		= "EFI block driver",
+	.id		= UCLASS_EFI,
+	.ops		= &driver_ops,
+};
diff --git a/lib/efi_driver/efi_uclass.c b/lib/efi_driver/efi_uclass.c
new file mode 100644
index 0000000000..90797f96d8
--- /dev/null
+++ b/lib/efi_driver/efi_uclass.c
@@ -0,0 +1,330 @@
+/*
+ *  Uclass for EFI drivers
+ *
+ *  Copyright (c) 2017 Heinrich Schuchardt
+ *
+ *  SPDX-License-Identifier:     GPL-2.0+
+ *
+ * For each EFI driver the uclass
+ * - creates a handle
+ * - installs the driver binding protocol
+ *
+ * The uclass provides the bind, start, and stop entry points for the driver
+ * binding protocol.
+ *
+ * In bind() and stop() it checks if the controller implements the protocol
+ * supported by the EFI driver. In the start() function it calls the bind()
+ * function of the EFI driver. In the stop() function it destroys the child
+ * controllers.
+ */
+
+#include <efi_driver.h>
+
+/*
+ * Check node type. We do not support partitions as controller handles.
+ *
+ * @handle	handle to be checked
+ * @return	status code
+ */
+static efi_status_t check_node_type(efi_handle_t handle)
+{
+	efi_status_t r, ret = EFI_SUCCESS;
+	const struct efi_device_path *dp;
+
+	/* Open the device path protocol */
+	r = EFI_CALL(systab.boottime->open_protocol(
+			handle, &efi_guid_device_path, (void **)&dp,
+			NULL, NULL, EFI_OPEN_PROTOCOL_GET_PROTOCOL));
+	if (r == EFI_SUCCESS && dp) {
+		/* Get the last node */
+		const struct efi_device_path *node = efi_dp_last_node(dp);
+		/* We do not support partitions as controller */
+		if (!node || node->type == DEVICE_PATH_TYPE_MEDIA_DEVICE)
+			ret = EFI_UNSUPPORTED;
+	}
+	return ret;
+}
+
+/*
+ * Check if the driver supports the controller.
+ *
+ * @this			driver binding protocol
+ * @controller_handle		handle of the controller
+ * @remaining_device_path	path specifying the child controller
+ * @return			status code
+ */
+static efi_status_t EFIAPI efi_uc_supported(
+		struct efi_driver_binding_protocol *this,
+		efi_handle_t controller_handle,
+		struct efi_device_path *remaining_device_path)
+{
+	efi_status_t r, ret;
+	void *interface;
+	struct efi_driver_binding_extended_protocol *bp =
+			(struct efi_driver_binding_extended_protocol *)this;
+
+	EFI_ENTRY("%p, %p, %ls", this, controller_handle,
+		  efi_dp_str(remaining_device_path));
+
+	ret = EFI_CALL(systab.boottime->open_protocol(
+			controller_handle, bp->ops->protocol,
+			&interface, this->driver_binding_handle,
+			controller_handle, EFI_OPEN_PROTOCOL_BY_DRIVER));
+	switch (ret) {
+	case EFI_ACCESS_DENIED:
+	case EFI_ALREADY_STARTED:
+		goto out;
+	case EFI_SUCCESS:
+		break;
+	default:
+		ret = EFI_UNSUPPORTED;
+		goto out;
+	}
+
+	ret = check_node_type(controller_handle);
+
+	r = EFI_CALL(systab.boottime->close_protocol(
+				controller_handle, bp->ops->protocol,
+				this->driver_binding_handle,
+				controller_handle));
+	if (r != EFI_SUCCESS)
+		ret = EFI_UNSUPPORTED;
+out:
+	return EFI_EXIT(ret);
+}
+
+/*
+ * Create child controllers and attach driver.
+ *
+ * @this			driver binding protocol
+ * @controller_handle		handle of the controller
+ * @remaining_device_path	path specifying the child controller
+ * @return			status code
+ */
+static efi_status_t EFIAPI efi_uc_start(
+		struct efi_driver_binding_protocol *this,
+		efi_handle_t controller_handle,
+		struct efi_device_path *remaining_device_path)
+{
+	efi_status_t r, ret;
+	void *interface = NULL;
+	struct efi_driver_binding_extended_protocol *bp =
+			(struct efi_driver_binding_extended_protocol *)this;
+
+	EFI_ENTRY("%p, %pUl, %ls", this, controller_handle,
+		  efi_dp_str(remaining_device_path));
+
+	/* Attach driver to controller */
+	ret = EFI_CALL(systab.boottime->open_protocol(
+			controller_handle, bp->ops->protocol,
+			&interface, this->driver_binding_handle,
+			controller_handle, EFI_OPEN_PROTOCOL_BY_DRIVER));
+	switch (ret) {
+	case EFI_ACCESS_DENIED:
+	case EFI_ALREADY_STARTED:
+		goto out;
+	case EFI_SUCCESS:
+		break;
+	default:
+		ret =  EFI_UNSUPPORTED;
+		goto out;
+	}
+	ret = check_node_type(controller_handle);
+	if (ret != EFI_SUCCESS) {
+		r = EFI_CALL(systab.boottime->close_protocol(
+				controller_handle, bp->ops->protocol,
+				this->driver_binding_handle,
+				controller_handle));
+		if (r != EFI_SUCCESS)
+			EFI_PRINT("Failure to close handle\n");
+		goto out;
+	}
+
+	/* TODO: driver specific stuff */
+	bp->ops->bind(controller_handle, interface);
+
+out:
+	return EFI_EXIT(ret);
+}
+
+/*
+ * Remove a single child controller from the parent controller.
+ *
+ * @controller_handle	parent controller
+ * @child_handle	child controller
+ * @return		status code
+ */
+static efi_status_t disconnect_child(efi_handle_t controller_handle,
+				     efi_handle_t child_handle)
+{
+	efi_status_t ret;
+	efi_guid_t *guid_controller = NULL;
+	efi_guid_t *guid_child_controller = NULL;
+
+	ret = EFI_CALL(systab.boottime->close_protocol(
+				controller_handle, guid_controller,
+				child_handle, child_handle));
+	if (ret != EFI_SUCCESS) {
+		EFI_PRINT("Cannot close protocol\n");
+		return ret;
+	}
+	ret = EFI_CALL(systab.boottime->uninstall_protocol_interface(
+				child_handle, guid_child_controller, NULL));
+	if (ret != EFI_SUCCESS) {
+		EFI_PRINT("Cannot uninstall protocol interface\n");
+		return ret;
+	}
+	return ret;
+}
+
+/*
+ * Remove child controllers and disconnect the controller.
+ *
+ * @this			driver binding protocol
+ * @controller_handle		handle of the controller
+ * @number_of_children		number of child controllers to remove
+ * @child_handle_buffer		handles of the child controllers to remove
+ * @return			status code
+ */
+static efi_status_t EFIAPI efi_uc_stop(
+		struct efi_driver_binding_protocol *this,
+		efi_handle_t controller_handle,
+		size_t number_of_children,
+		efi_handle_t *child_handle_buffer)
+{
+	efi_status_t ret;
+	efi_uintn_t count;
+	struct efi_open_protocol_info_entry *entry_buffer;
+	efi_guid_t *guid_controller = NULL;
+
+	EFI_ENTRY("%p, %pUl, %zu, %p", this, controller_handle,
+		  number_of_children, child_handle_buffer);
+
+	/* Destroy provided child controllers */
+	if (number_of_children) {
+		efi_uintn_t i;
+
+		for (i = 0; i < number_of_children; ++i) {
+			ret = disconnect_child(controller_handle,
+					       child_handle_buffer[i]);
+			if (ret != EFI_SUCCESS)
+				return ret;
+		}
+		return EFI_SUCCESS;
+	}
+
+	/* Destroy all children */
+	ret = EFI_CALL(systab.boottime->open_protocol_information(
+					controller_handle, guid_controller,
+					&entry_buffer, &count));
+	if (ret != EFI_SUCCESS)
+		goto out;
+	while (count) {
+		if (entry_buffer[--count].attributes &
+		    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER) {
+			ret = disconnect_child(
+					controller_handle,
+					entry_buffer[count].agent_handle);
+			if (ret != EFI_SUCCESS)
+				goto out;
+		}
+	}
+	ret = EFI_CALL(systab.boottime->free_pool(entry_buffer));
+	if (ret != EFI_SUCCESS)
+		printf("%s(%u) %s: ERROR: Cannot free pool\n",
+		       __FILE__, __LINE__, __func__);
+
+	/* Detach driver from controller */
+	ret = EFI_CALL(systab.boottime->close_protocol(
+			controller_handle, guid_controller,
+			this->driver_binding_handle, controller_handle));
+out:
+	return EFI_EXIT(ret);
+}
+
+static efi_status_t efi_add_driver(struct driver *drv)
+{
+	efi_status_t ret;
+	const struct efi_driver_ops *ops = drv->ops;
+	struct efi_driver_binding_extended_protocol *bp;
+
+	debug("EFI: Adding driver '%s'\n", drv->name);
+	if (!ops->protocol) {
+		printf("EFI: ERROR: protocol GUID missing for driver '%s'\n",
+		       drv->name);
+		return EFI_INVALID_PARAMETER;
+	}
+	bp = calloc(1, sizeof(struct efi_driver_binding_extended_protocol));
+	if (!bp)
+		return EFI_OUT_OF_RESOURCES;
+
+	bp->bp.supported = efi_uc_supported;
+	bp->bp.start = efi_uc_start;
+	bp->bp.stop = efi_uc_stop;
+	bp->bp.version = 0xffffffff;
+	bp->ops = drv->ops;
+
+	ret = efi_create_handle(&bp->bp.driver_binding_handle);
+	if (ret != EFI_SUCCESS) {
+		free(bp);
+		goto out;
+	}
+	bp->bp.image_handle = bp->bp.driver_binding_handle;
+	ret = efi_add_protocol(bp->bp.driver_binding_handle,
+			       &efi_guid_driver_binding_protocol, bp);
+	if (ret != EFI_SUCCESS) {
+		efi_delete_handle(bp->bp.driver_binding_handle);
+		free(bp);
+		goto out;
+	}
+out:
+	return ret;
+}
+
+/*
+ * Initialize the EFI drivers.
+ * Called by board_init_r().
+ *
+ * @return	0 = success, any other value will stop further execution
+ */
+int efi_driver_init(void)
+{
+	struct driver *drv;
+	int ret = 0;
+
+	/* Save 'gd' pointer */
+	efi_save_gd();
+
+	debug("EFI: Initializing EFI driver framework\n");
+	for (drv = ll_entry_start(struct driver, driver);
+	     drv < ll_entry_end(struct driver, driver); ++drv) {
+		if (drv->id == UCLASS_EFI) {
+			ret = efi_add_driver(drv);
+			if (ret) {
+				printf("EFI: ERROR: failed to add driver %s\n",
+				       drv->name);
+				break;
+			}
+		}
+	}
+	return ret;
+}
+
+static int efi_uc_init(struct uclass *class)
+{
+	printf("EFI: Initializing UCLASS_EFI\n");
+	return 0;
+}
+
+static int efi_uc_destroy(struct uclass *class)
+{
+	printf("Destroying  UCLASS_EFI\n");
+	return 0;
+}
+
+UCLASS_DRIVER(efi) = {
+	.name		= "efi",
+	.id		= UCLASS_EFI,
+	.init		= efi_uc_init,
+	.destroy	= efi_uc_destroy,
+};
diff --git a/lib/efi_loader/efi_boottime.c b/lib/efi_loader/efi_boottime.c
index b90bd0b426..39d8511fe3 100644
--- a/lib/efi_loader/efi_boottime.c
+++ b/lib/efi_loader/efi_boottime.c
@@ -56,6 +56,14 @@ static volatile void *efi_gd, *app_gd;
 
 static int entry_count;
 static int nesting_level;
+/* GUID of the EFI_DRIVER_BINDING_PROTOCOL */
+const efi_guid_t efi_guid_driver_binding_protocol =
+			EFI_DRIVER_BINDING_PROTOCOL_GUID;
+
+static efi_status_t EFIAPI efi_disconnect_controller(
+					efi_handle_t controller_handle,
+					efi_handle_t driver_image_handle,
+					efi_handle_t child_handle);
 
 /* Called on every callback entry */
 int __efi_entry_check(void)
@@ -141,13 +149,14 @@ const char *__efi_nesting_dec(void)
  * For the SignalEvent service see efi_signal_event_ext.
  *
  * @event	event to signal
+ * @check_tpl	check the TPL level
  */
-void efi_signal_event(struct efi_event *event)
+void efi_signal_event(struct efi_event *event, bool check_tpl)
 {
 	if (event->notify_function) {
 		event->is_queued = true;
 		/* Check TPL */
-		if (efi_tpl >= event->notify_tpl)
+		if (check_tpl && efi_tpl >= event->notify_tpl)
 			return;
 		EFI_CALL_VOID(event->notify_function(event,
 						     event->notify_context));
@@ -344,7 +353,7 @@ void efi_add_handle(struct efi_object *obj)
  * @handle	new handle
  * @return	status code
  */
-efi_status_t efi_create_handle(void **handle)
+efi_status_t efi_create_handle(efi_handle_t *handle)
 {
 	struct efi_object *obj;
 	efi_status_t r;
@@ -367,7 +376,7 @@ efi_status_t efi_create_handle(void **handle)
  * @handler		reference to the protocol
  * @return		status code
  */
-efi_status_t efi_search_protocol(const void *handle,
+efi_status_t efi_search_protocol(const efi_handle_t handle,
 				 const efi_guid_t *protocol_guid,
 				 struct efi_handler **handler)
 {
@@ -400,7 +409,8 @@ efi_status_t efi_search_protocol(const void *handle,
  * @protocol_interface		interface of the protocol implementation
  * @return			status code
  */
-efi_status_t efi_remove_protocol(const void *handle, const efi_guid_t *protocol,
+efi_status_t efi_remove_protocol(const efi_handle_t handle,
+				 const efi_guid_t *protocol,
 				 void *protocol_interface)
 {
 	struct efi_handler *handler;
@@ -422,21 +432,18 @@ efi_status_t efi_remove_protocol(const void *handle, const efi_guid_t *protocol,
  * @handle	handle from which the protocols shall be deleted
  * @return	status code
  */
-efi_status_t efi_remove_all_protocols(const void *handle)
+efi_status_t efi_remove_all_protocols(const efi_handle_t handle)
 {
 	struct efi_object *efiobj;
-	struct list_head *lhandle;
-	struct list_head *pos;
+	struct efi_handler *protocol;
+	struct efi_handler *pos;
 
 	efiobj = efi_search_obj(handle);
 	if (!efiobj)
 		return EFI_INVALID_PARAMETER;
-	list_for_each_safe(lhandle, pos, &efiobj->protocols) {
-		struct efi_handler *protocol;
+	list_for_each_entry_safe(protocol, pos, &efiobj->protocols, link) {
 		efi_status_t ret;
 
-		protocol = list_entry(lhandle, struct efi_handler, link);
-
 		ret = efi_remove_protocol(handle, protocol->guid,
 					  protocol->protocol_interface);
 		if (ret != EFI_SUCCESS)
@@ -559,7 +566,7 @@ void efi_timer_check(void)
 		if (!efi_events[i].type)
 			continue;
 		if (efi_events[i].is_queued)
-			efi_signal_event(&efi_events[i]);
+			efi_signal_event(&efi_events[i], true);
 		if (!(efi_events[i].type & EVT_TIMER) ||
 		    now < efi_events[i].trigger_next)
 			continue;
@@ -575,7 +582,7 @@ void efi_timer_check(void)
 			continue;
 		}
 		efi_events[i].is_signaled = true;
-		efi_signal_event(&efi_events[i]);
+		efi_signal_event(&efi_events[i], true);
 	}
 	WATCHDOG_RESET();
 }
@@ -684,7 +691,7 @@ known_event:
 		if (!event[i]->type || event[i]->type & EVT_NOTIFY_SIGNAL)
 			return EFI_EXIT(EFI_INVALID_PARAMETER);
 		if (!event[i]->is_signaled)
-			efi_signal_event(event[i]);
+			efi_signal_event(event[i], true);
 	}
 
 	/* Wait for signal */
@@ -734,7 +741,7 @@ static efi_status_t EFIAPI efi_signal_event_ext(struct efi_event *event)
 			break;
 		event->is_signaled = true;
 		if (event->type & EVT_NOTIFY_SIGNAL)
-			efi_signal_event(event);
+			efi_signal_event(event, true);
 		break;
 	}
 	return EFI_EXIT(EFI_SUCCESS);
@@ -791,7 +798,7 @@ static efi_status_t EFIAPI efi_check_event(struct efi_event *event)
 		if (!event->type || event->type & EVT_NOTIFY_SIGNAL)
 			break;
 		if (!event->is_signaled)
-			efi_signal_event(event);
+			efi_signal_event(event, true);
 		if (event->is_signaled)
 			return EFI_EXIT(EFI_SUCCESS);
 		return EFI_EXIT(EFI_NOT_READY);
@@ -805,7 +812,7 @@ static efi_status_t EFIAPI efi_check_event(struct efi_event *event)
  * @handle	handle to find
  * @return	EFI object
  */
-struct efi_object *efi_search_obj(const void *handle)
+struct efi_object *efi_search_obj(const efi_handle_t handle)
 {
 	struct efi_object *efiobj;
 
@@ -818,6 +825,40 @@ struct efi_object *efi_search_obj(const void *handle)
 }
 
 /*
+ * Create open protocol info entry and add it to a protocol.
+ *
+ * @handler	handler of a protocol
+ * @return	open protocol info entry
+ */
+static struct efi_open_protocol_info_entry *efi_create_open_info(
+			struct efi_handler *handler)
+{
+	struct efi_open_protocol_info_item *item;
+
+	item = calloc(1, sizeof(struct efi_open_protocol_info_item));
+	if (!item)
+		return NULL;
+	/* Append the item to the open protocol info list. */
+	list_add_tail(&item->link, &handler->open_infos);
+
+	return &item->info;
+}
+
+/*
+ * Remove an open protocol info entry from a protocol.
+ *
+ * @handler	handler of a protocol
+ * @return	status code
+ */
+static efi_status_t efi_delete_open_info(
+			struct efi_open_protocol_info_item *item)
+{
+	list_del(&item->link);
+	free(item);
+	return EFI_SUCCESS;
+}
+
+/*
  * Install new protocol on a handle.
  *
  * @handle			handle on which the protocol shall be installed
@@ -825,7 +866,8 @@ struct efi_object *efi_search_obj(const void *handle)
  * @protocol_interface		interface of the protocol implementation
  * @return			status code
  */
-efi_status_t efi_add_protocol(const void *handle, const efi_guid_t *protocol,
+efi_status_t efi_add_protocol(const efi_handle_t handle,
+			      const efi_guid_t *protocol,
 			      void *protocol_interface)
 {
 	struct efi_object *efiobj;
@@ -843,7 +885,10 @@ efi_status_t efi_add_protocol(const void *handle, const efi_guid_t *protocol,
 		return EFI_OUT_OF_RESOURCES;
 	handler->guid = protocol;
 	handler->protocol_interface = protocol_interface;
+	INIT_LIST_HEAD(&handler->open_infos);
 	list_add_tail(&handler->link, &efiobj->protocols);
+	if (!guidcmp(&efi_guid_device_path, protocol))
+		EFI_PRINT("installed device path '%pD'\n", protocol_interface);
 	return EFI_SUCCESS;
 }
 
@@ -907,9 +952,9 @@ out:
  * @new_interface		interface to be installed
  * @return			status code
  */
-static efi_status_t EFIAPI efi_reinstall_protocol_interface(void *handle,
-			const efi_guid_t *protocol, void *old_interface,
-			void *new_interface)
+static efi_status_t EFIAPI efi_reinstall_protocol_interface(
+			efi_handle_t handle, const efi_guid_t *protocol,
+			void *old_interface, void *new_interface)
 {
 	EFI_ENTRY("%p, %pUl, %p, %p", handle, protocol, old_interface,
 		  new_interface);
@@ -917,6 +962,109 @@ static efi_status_t EFIAPI efi_reinstall_protocol_interface(void *handle,
 }
 
 /*
+ * Get all drivers associated to a controller.
+ * The allocated buffer has to be freed with free().
+ *
+ * @efiobj			handle of the controller
+ * @protocol			protocol guid (optional)
+ * @number_of_drivers		number of child controllers
+ * @driver_handle_buffer	handles of the the drivers
+ * @return			status code
+ */
+static efi_status_t efi_get_drivers(struct efi_object *efiobj,
+				    const efi_guid_t *protocol,
+				    efi_uintn_t *number_of_drivers,
+				    efi_handle_t **driver_handle_buffer)
+{
+	struct efi_handler *handler;
+	struct efi_open_protocol_info_item *item;
+	efi_uintn_t count = 0, i;
+	bool duplicate;
+
+	/* Count all driver associations */
+	list_for_each_entry(handler, &efiobj->protocols, link) {
+		if (protocol && guidcmp(handler->guid, protocol))
+			continue;
+		list_for_each_entry(item, &handler->open_infos, link) {
+			if (item->info.attributes &
+			    EFI_OPEN_PROTOCOL_BY_DRIVER)
+				++count;
+		}
+	}
+	/*
+	 * Create buffer. In case of duplicate driver assignments the buffer
+	 * will be too large. But that does not harm.
+	 */
+	*number_of_drivers = 0;
+	*driver_handle_buffer = calloc(count, sizeof(efi_handle_t));
+	if (!*driver_handle_buffer)
+		return EFI_OUT_OF_RESOURCES;
+	/* Collect unique driver handles */
+	list_for_each_entry(handler, &efiobj->protocols, link) {
+		if (protocol && guidcmp(handler->guid, protocol))
+			continue;
+		list_for_each_entry(item, &handler->open_infos, link) {
+			if (item->info.attributes &
+			    EFI_OPEN_PROTOCOL_BY_DRIVER) {
+				/* Check this is a new driver */
+				duplicate = false;
+				for (i = 0; i < *number_of_drivers; ++i) {
+					if ((*driver_handle_buffer)[i] ==
+					    item->info.agent_handle)
+						duplicate = true;
+				}
+				/* Copy handle to buffer */
+				if (!duplicate) {
+					i = (*number_of_drivers)++;
+					(*driver_handle_buffer)[i] =
+						item->info.agent_handle;
+				}
+			}
+		}
+	}
+	return EFI_SUCCESS;
+}
+
+/*
+ * Disconnect all drivers from a controller.
+ *
+ * This function implements the DisconnectController service.
+ * See the Unified Extensible Firmware Interface (UEFI) specification
+ * for details.
+ *
+ * @efiobj		handle of the controller
+ * @protocol		protocol guid (optional)
+ * @child_handle	handle of the child to destroy
+ * @return		status code
+ */
+static efi_status_t efi_disconnect_all_drivers(
+				struct efi_object *efiobj,
+				const efi_guid_t *protocol,
+				efi_handle_t child_handle)
+{
+	efi_uintn_t number_of_drivers;
+	efi_handle_t *driver_handle_buffer;
+	efi_status_t r, ret;
+
+	ret = efi_get_drivers(efiobj, protocol, &number_of_drivers,
+			      &driver_handle_buffer);
+	if (ret != EFI_SUCCESS)
+		return ret;
+
+	ret = EFI_NOT_FOUND;
+	while (number_of_drivers) {
+		r = EFI_CALL(efi_disconnect_controller(
+				efiobj->handle,
+				driver_handle_buffer[--number_of_drivers],
+				child_handle));
+		if (r == EFI_SUCCESS)
+			ret = r;
+	}
+	free(driver_handle_buffer);
+	return ret;
+}
+
+/*
  * Uninstall protocol interface.
  *
  * This function implements the UninstallProtocolInterface service.
@@ -929,29 +1077,46 @@ static efi_status_t EFIAPI efi_reinstall_protocol_interface(void *handle,
  * @return			status code
  */
 static efi_status_t EFIAPI efi_uninstall_protocol_interface(
-				void *handle, const efi_guid_t *protocol,
+				efi_handle_t handle, const efi_guid_t *protocol,
 				void *protocol_interface)
 {
+	struct efi_object *efiobj;
 	struct efi_handler *handler;
+	struct efi_open_protocol_info_item *item;
+	struct efi_open_protocol_info_item *pos;
 	efi_status_t r;
 
 	EFI_ENTRY("%p, %pUl, %p", handle, protocol, protocol_interface);
 
-	if (!handle || !protocol) {
+	/* Check handle */
+	efiobj = efi_search_obj(handle);
+	if (!efiobj) {
 		r = EFI_INVALID_PARAMETER;
 		goto out;
 	}
-
 	/* Find the protocol on the handle */
 	r = efi_search_protocol(handle, protocol, &handler);
 	if (r != EFI_SUCCESS)
 		goto out;
-	if (handler->protocol_interface) {
-		/* TODO disconnect controllers */
+	/* Disconnect controllers */
+	efi_disconnect_all_drivers(efiobj, protocol, NULL);
+	if (!list_empty(&handler->open_infos)) {
 		r =  EFI_ACCESS_DENIED;
-	} else {
-		r = efi_remove_protocol(handle, protocol, protocol_interface);
+		goto out;
+	}
+	/* Close protocol */
+	list_for_each_entry_safe(item, pos, &handler->open_infos, link) {
+		if (item->info.attributes ==
+			EFI_OPEN_PROTOCOL_BY_HANDLE_PROTOCOL ||
+		    item->info.attributes == EFI_OPEN_PROTOCOL_GET_PROTOCOL ||
+		    item->info.attributes == EFI_OPEN_PROTOCOL_TEST_PROTOCOL)
+			list_del(&item->link);
 	}
+	if (!list_empty(&handler->open_infos)) {
+		r =  EFI_ACCESS_DENIED;
+		goto out;
+	}
+	r = efi_remove_protocol(handle, protocol, protocol_interface);
 out:
 	return EFI_EXIT(r);
 }
@@ -1310,7 +1475,7 @@ static efi_status_t EFIAPI efi_load_image(bool boot_policy,
 	struct efi_object *obj;
 	efi_status_t ret;
 
-	EFI_ENTRY("%d, %p, %p, %p, %ld, %p", boot_policy, parent_image,
+	EFI_ENTRY("%d, %p, %pD, %p, %ld, %p", boot_policy, parent_image,
 		  file_path, source_buffer, source_size, image_handle);
 
 	info = calloc(1, sizeof(*info));
@@ -1369,8 +1534,10 @@ static efi_status_t EFIAPI efi_start_image(efi_handle_t image_handle,
 					   unsigned long *exit_data_size,
 					   s16 **exit_data)
 {
-	ulong (*entry)(void *image_handle, struct efi_system_table *st);
+	asmlinkage ulong (*entry)(efi_handle_t image_handle,
+				  struct efi_system_table *st);
 	struct efi_loaded_image *info = image_handle;
+	efi_status_t ret;
 
 	EFI_ENTRY("%p, %p, %p", image_handle, exit_data_size, exit_data);
 	entry = info->reserved;
@@ -1379,18 +1546,37 @@ static efi_status_t EFIAPI efi_start_image(efi_handle_t image_handle,
 
 	/* call the image! */
 	if (setjmp(&info->exit_jmp)) {
-		/* We returned from the child image */
+		/*
+		 * We called the entry point of the child image with EFI_CALL
+		 * in the lines below. The child image called the Exit() boot
+		 * service efi_exit() which executed the long jump that brought
+		 * us to the current line. This implies that the second half
+		 * of the EFI_CALL macro has not been executed.
+		 */
+#ifdef CONFIG_ARM
+		/*
+		 * efi_exit() called efi_restore_gd(). We have to undo this
+		 * otherwise __efi_entry_check() will put the wrong value into
+		 * app_gd.
+		 */
+		gd = app_gd;
+#endif
+		/*
+		 * To get ready to call EFI_EXIT below we have to execute the
+		 * missed out steps of EFI_CALL.
+		 */
+		assert(__efi_entry_check());
+		debug("%sEFI: %lu returned by started image\n",
+		      __efi_nesting_dec(),
+		      (unsigned long)((uintptr_t)info->exit_status &
+				      ~EFI_ERROR_MASK));
 		return EFI_EXIT(info->exit_status);
 	}
 
-	__efi_nesting_dec();
-	__efi_exit_check();
-	entry(image_handle, &systab);
-	__efi_entry_check();
-	__efi_nesting_inc();
+	ret = EFI_CALL(entry(image_handle, &systab));
 
 	/* Should usually never get here */
-	return EFI_EXIT(EFI_SUCCESS);
+	return EFI_EXIT(ret);
 }
 
 /*
@@ -1427,7 +1613,7 @@ static efi_status_t EFIAPI efi_exit(efi_handle_t image_handle,
 		  exit_data_size, exit_data);
 
 	/* Make sure entry/exit counts for EFI world cross-overs match */
-	__efi_exit_check();
+	EFI_EXIT(exit_status);
 
 	/*
 	 * But longjmp out with the U-Boot gd, not the application's, as
@@ -1451,7 +1637,7 @@ static efi_status_t EFIAPI efi_exit(efi_handle_t image_handle,
  * @image_handle	handle of the image to be unloaded
  * @return		status code
  */
-static efi_status_t EFIAPI efi_unload_image(void *image_handle)
+static efi_status_t EFIAPI efi_unload_image(efi_handle_t image_handle)
 {
 	struct efi_object *efiobj;
 
@@ -1479,33 +1665,43 @@ static void efi_exit_caches(void)
 }
 
 /*
- * Stop boot services.
+ * Stop all boot services.
  *
  * This function implements the ExitBootServices service.
  * See the Unified Extensible Firmware Interface (UEFI) specification
  * for details.
  *
+ * All timer events are disabled.
+ * For exit boot services events the notification function is called.
+ * The boot services are disabled in the system table.
+ *
  * @image_handle	handle of the loaded image
  * @map_key		key of the memory map
  * @return		status code
  */
-static efi_status_t EFIAPI efi_exit_boot_services(void *image_handle,
+static efi_status_t EFIAPI efi_exit_boot_services(efi_handle_t image_handle,
 						  unsigned long map_key)
 {
 	int i;
 
 	EFI_ENTRY("%p, %ld", image_handle, map_key);
 
+	/* Make sure that notification functions are not called anymore */
+	efi_tpl = TPL_HIGH_LEVEL;
+
+	/* Check if ExitBootServices has already been called */
+	if (!systab.boottime)
+		return EFI_EXIT(EFI_SUCCESS);
+
 	/* Notify that ExitBootServices is invoked. */
 	for (i = 0; i < ARRAY_SIZE(efi_events); ++i) {
 		if (efi_events[i].type != EVT_SIGNAL_EXIT_BOOT_SERVICES)
 			continue;
-		efi_signal_event(&efi_events[i]);
+		efi_events[i].is_signaled = true;
+		efi_signal_event(&efi_events[i], false);
 	}
-	/* Make sure that notification functions are not called anymore */
-	efi_tpl = TPL_HIGH_LEVEL;
 
-	/* XXX Should persist EFI variables here */
+	/* TODO Should persist EFI variables here */
 
 	board_quiesce_devices();
 
@@ -1515,6 +1711,20 @@ static efi_status_t EFIAPI efi_exit_boot_services(void *image_handle,
 	/* This stops all lingering devices */
 	bootm_disable_interrupts();
 
+	/* Disable boottime services */
+	systab.con_in_handle = NULL;
+	systab.con_in = NULL;
+	systab.con_out_handle = NULL;
+	systab.con_out = NULL;
+	systab.stderr_handle = NULL;
+	systab.std_err = NULL;
+	systab.boottime = NULL;
+
+	/* Recalculate CRC32 */
+	systab.hdr.crc32 = 0;
+	systab.hdr.crc32 = crc32(0, (const unsigned char *)&systab,
+				 sizeof(struct efi_system_table));
+
 	/* Give the payload some time to boot */
 	efi_set_watchdog(0);
 	WATCHDOG_RESET();
@@ -1581,51 +1791,6 @@ static efi_status_t EFIAPI efi_set_watchdog_timer(unsigned long timeout,
 }
 
 /*
- * Connect a controller to a driver.
- *
- * This function implements the ConnectController service.
- * See the Unified Extensible Firmware Interface (UEFI) specification
- * for details.
- *
- * @controller_handle	handle of the controller
- * @driver_image_handle	handle of the driver
- * @remain_device_path	device path of a child controller
- * @recursive		true to connect all child controllers
- * @return		status code
- */
-static efi_status_t EFIAPI efi_connect_controller(
-			efi_handle_t controller_handle,
-			efi_handle_t *driver_image_handle,
-			struct efi_device_path *remain_device_path,
-			bool recursive)
-{
-	EFI_ENTRY("%p, %p, %p, %d", controller_handle, driver_image_handle,
-		  remain_device_path, recursive);
-	return EFI_EXIT(EFI_NOT_FOUND);
-}
-
-/*
- * Disconnect a controller from a driver.
- *
- * This function implements the DisconnectController service.
- * See the Unified Extensible Firmware Interface (UEFI) specification
- * for details.
- *
- * @controller_handle	handle of the controller
- * @driver_image_handle handle of the driver
- * @child_handle	handle of the child to destroy
- * @return		status code
- */
-static efi_status_t EFIAPI efi_disconnect_controller(void *controller_handle,
-						     void *driver_image_handle,
-						     void *child_handle)
-{
-	EFI_ENTRY("%p, %p, %p", controller_handle, driver_image_handle,
-		  child_handle);
-	return EFI_EXIT(EFI_INVALID_PARAMETER);
-}
-
-/*
  * Close a protocol.
  *
  * This function implements the CloseProtocol service.
@@ -1638,14 +1803,38 @@ static efi_status_t EFIAPI efi_disconnect_controller(void *controller_handle,
  * @controller_handle	handle of the controller
  * @return		status code
  */
-static efi_status_t EFIAPI efi_close_protocol(void *handle,
+static efi_status_t EFIAPI efi_close_protocol(efi_handle_t handle,
 					      const efi_guid_t *protocol,
-					      void *agent_handle,
-					      void *controller_handle)
+					      efi_handle_t agent_handle,
+					      efi_handle_t controller_handle)
 {
+	struct efi_handler *handler;
+	struct efi_open_protocol_info_item *item;
+	struct efi_open_protocol_info_item *pos;
+	efi_status_t r;
+
 	EFI_ENTRY("%p, %pUl, %p, %p", handle, protocol, agent_handle,
 		  controller_handle);
-	return EFI_EXIT(EFI_NOT_FOUND);
+
+	if (!agent_handle) {
+		r = EFI_INVALID_PARAMETER;
+		goto out;
+	}
+	r = efi_search_protocol(handle, protocol, &handler);
+	if (r != EFI_SUCCESS)
+		goto out;
+
+	r = EFI_NOT_FOUND;
+	list_for_each_entry_safe(item, pos, &handler->open_infos, link) {
+		if (item->info.agent_handle == agent_handle &&
+		    item->info.controller_handle == controller_handle) {
+			efi_delete_open_info(item);
+			r = EFI_SUCCESS;
+			break;
+		}
+	}
+out:
+	return EFI_EXIT(r);
 }
 
 /*
@@ -1666,9 +1855,49 @@ static efi_status_t EFIAPI efi_open_protocol_information(efi_handle_t handle,
 			struct efi_open_protocol_info_entry **entry_buffer,
 			efi_uintn_t *entry_count)
 {
+	unsigned long buffer_size;
+	unsigned long count;
+	struct efi_handler *handler;
+	struct efi_open_protocol_info_item *item;
+	efi_status_t r;
+
 	EFI_ENTRY("%p, %pUl, %p, %p", handle, protocol, entry_buffer,
 		  entry_count);
-	return EFI_EXIT(EFI_NOT_FOUND);
+
+	/* Check parameters */
+	if (!entry_buffer) {
+		r = EFI_INVALID_PARAMETER;
+		goto out;
+	}
+	r = efi_search_protocol(handle, protocol, &handler);
+	if (r != EFI_SUCCESS)
+		goto out;
+
+	/* Count entries */
+	count = 0;
+	list_for_each_entry(item, &handler->open_infos, link) {
+		if (item->info.open_count)
+			++count;
+	}
+	*entry_count = count;
+	*entry_buffer = NULL;
+	if (!count) {
+		r = EFI_SUCCESS;
+		goto out;
+	}
+
+	/* Copy entries */
+	buffer_size = count * sizeof(struct efi_open_protocol_info_entry);
+	r = efi_allocate_pool(EFI_ALLOCATE_ANY_PAGES, buffer_size,
+			      (void **)entry_buffer);
+	if (r != EFI_SUCCESS)
+		goto out;
+	list_for_each_entry_reverse(item, &handler->open_infos, link) {
+		if (item->info.open_count)
+			(*entry_buffer)[--count] = item->info;
+	}
+out:
+	return EFI_EXIT(r);
 }
 
 /*
@@ -1683,8 +1912,8 @@ static efi_status_t EFIAPI efi_open_protocol_information(efi_handle_t handle,
  * @protocol_buffer_count	number of entries in the buffer
  * @return			status code
  */
-static efi_status_t EFIAPI efi_protocols_per_handle(void *handle,
-			efi_guid_t ***protocol_buffer,
+static efi_status_t EFIAPI efi_protocols_per_handle(
+			efi_handle_t handle, efi_guid_t ***protocol_buffer,
 			efi_uintn_t *protocol_buffer_count)
 {
 	unsigned long buffer_size;
@@ -1774,7 +2003,7 @@ static efi_status_t EFIAPI efi_locate_handle_buffer(
 	r = efi_locate_handle(search_type, protocol, search_key, &buffer_size,
 			      *buffer);
 	if (r == EFI_SUCCESS)
-		*no_handles = buffer_size / sizeof(void *);
+		*no_handles = buffer_size / sizeof(efi_handle_t);
 out:
 	return EFI_EXIT(r);
 }
@@ -2071,6 +2300,101 @@ static void EFIAPI efi_set_mem(void *buffer, size_t size, uint8_t value)
 /*
  * Open protocol interface on a handle.
  *
+ * @handler		handler of a protocol
+ * @protocol_interface	interface implementing the protocol
+ * @agent_handle	handle of the driver
+ * @controller_handle	handle of the controller
+ * @attributes		attributes indicating how to open the protocol
+ * @return		status code
+ */
+static efi_status_t efi_protocol_open(
+			struct efi_handler *handler,
+			void **protocol_interface, void *agent_handle,
+			void *controller_handle, uint32_t attributes)
+{
+	struct efi_open_protocol_info_item *item;
+	struct efi_open_protocol_info_entry *match = NULL;
+	bool opened_by_driver = false;
+	bool opened_exclusive = false;
+
+	/* If there is no agent, only return the interface */
+	if (!agent_handle)
+		goto out;
+
+	/* For TEST_PROTOCOL ignore interface attribute */
+	if (attributes != EFI_OPEN_PROTOCOL_TEST_PROTOCOL)
+		*protocol_interface = NULL;
+
+	/*
+	 * Check if the protocol is already opened by a driver with the same
+	 * attributes or opened exclusively
+	 */
+	list_for_each_entry(item, &handler->open_infos, link) {
+		if (item->info.agent_handle == agent_handle) {
+			if ((attributes & EFI_OPEN_PROTOCOL_BY_DRIVER) &&
+			    (item->info.attributes == attributes))
+				return EFI_ALREADY_STARTED;
+		}
+		if (item->info.attributes & EFI_OPEN_PROTOCOL_EXCLUSIVE)
+			opened_exclusive = true;
+	}
+
+	/* Only one controller can open the protocol exclusively */
+	if (opened_exclusive && attributes &
+	    (EFI_OPEN_PROTOCOL_EXCLUSIVE | EFI_OPEN_PROTOCOL_BY_DRIVER))
+		return EFI_ACCESS_DENIED;
+
+	/* Prepare exclusive opening */
+	if (attributes & EFI_OPEN_PROTOCOL_EXCLUSIVE) {
+		/* Try to disconnect controllers */
+		list_for_each_entry(item, &handler->open_infos, link) {
+			if (item->info.attributes ==
+					EFI_OPEN_PROTOCOL_BY_DRIVER)
+				EFI_CALL(efi_disconnect_controller(
+						item->info.controller_handle,
+						item->info.agent_handle,
+						NULL));
+		}
+		opened_by_driver = false;
+		/* Check if all controllers are disconnected */
+		list_for_each_entry(item, &handler->open_infos, link) {
+			if (item->info.attributes & EFI_OPEN_PROTOCOL_BY_DRIVER)
+				opened_by_driver = true;
+		}
+		/* Only one controller can be conncected */
+		if (opened_by_driver)
+			return EFI_ACCESS_DENIED;
+	}
+
+	/* Find existing entry */
+	list_for_each_entry(item, &handler->open_infos, link) {
+		if (item->info.agent_handle == agent_handle &&
+		    item->info.controller_handle == controller_handle)
+			match = &item->info;
+	}
+	/* None found, create one */
+	if (!match) {
+		match = efi_create_open_info(handler);
+		if (!match)
+			return EFI_OUT_OF_RESOURCES;
+	}
+
+	match->agent_handle = agent_handle;
+	match->controller_handle = controller_handle;
+	match->attributes = attributes;
+	match->open_count++;
+
+out:
+	/* For TEST_PROTOCOL ignore interface attribute. */
+	if (attributes != EFI_OPEN_PROTOCOL_TEST_PROTOCOL)
+		*protocol_interface = handler->protocol_interface;
+
+	return EFI_SUCCESS;
+}
+
+/*
+ * Open protocol interface on a handle.
+ *
  * This function implements the OpenProtocol interface.
  * See the Unified Extensible Firmware Interface (UEFI) specification
  * for details.
@@ -2109,12 +2433,16 @@ static efi_status_t EFIAPI efi_open_protocol(
 	case EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER:
 		if (controller_handle == handle)
 			goto out;
+		/* fall-through */
 	case EFI_OPEN_PROTOCOL_BY_DRIVER:
 	case EFI_OPEN_PROTOCOL_BY_DRIVER | EFI_OPEN_PROTOCOL_EXCLUSIVE:
-		if (controller_handle == NULL)
+		/* Check that the controller handle is valid */
+		if (!efi_search_obj(controller_handle))
 			goto out;
+		/* fall-through */
 	case EFI_OPEN_PROTOCOL_EXCLUSIVE:
-		if (agent_handle == NULL)
+		/* Check that the agent handle is valid */
+		if (!efi_search_obj(agent_handle))
 			goto out;
 		break;
 	default:
@@ -2125,8 +2453,8 @@ static efi_status_t EFIAPI efi_open_protocol(
 	if (r != EFI_SUCCESS)
 		goto out;
 
-	if (attributes != EFI_OPEN_PROTOCOL_TEST_PROTOCOL)
-		*protocol_interface = handler->protocol_interface;
+	r = efi_protocol_open(handler, protocol_interface, agent_handle,
+			      controller_handle, attributes);
 out:
 	return EFI_EXIT(r);
 }
@@ -2143,7 +2471,7 @@ out:
  * @protocol_interface  interface implementing the protocol
  * @return		status code
  */
-static efi_status_t EFIAPI efi_handle_protocol(void *handle,
+static efi_status_t EFIAPI efi_handle_protocol(efi_handle_t handle,
 					       const efi_guid_t *protocol,
 					       void **protocol_interface)
 {
@@ -2151,6 +2479,321 @@ static efi_status_t EFIAPI efi_handle_protocol(void *handle,
 				 NULL, EFI_OPEN_PROTOCOL_BY_HANDLE_PROTOCOL);
 }
 
+static efi_status_t efi_bind_controller(
+			efi_handle_t controller_handle,
+			efi_handle_t driver_image_handle,
+			struct efi_device_path *remain_device_path)
+{
+	struct efi_driver_binding_protocol *binding_protocol;
+	efi_status_t r;
+
+	r = EFI_CALL(efi_open_protocol(driver_image_handle,
+				       &efi_guid_driver_binding_protocol,
+				       (void **)&binding_protocol,
+				       driver_image_handle, NULL,
+				       EFI_OPEN_PROTOCOL_GET_PROTOCOL));
+	if (r != EFI_SUCCESS)
+		return r;
+	r = EFI_CALL(binding_protocol->supported(binding_protocol,
+						 controller_handle,
+						 remain_device_path));
+	if (r == EFI_SUCCESS)
+		r = EFI_CALL(binding_protocol->start(binding_protocol,
+						     controller_handle,
+						     remain_device_path));
+	EFI_CALL(efi_close_protocol(driver_image_handle,
+				    &efi_guid_driver_binding_protocol,
+				    driver_image_handle, NULL));
+	return r;
+}
+
+static efi_status_t efi_connect_single_controller(
+			efi_handle_t controller_handle,
+			efi_handle_t *driver_image_handle,
+			struct efi_device_path *remain_device_path)
+{
+	efi_handle_t *buffer;
+	size_t count;
+	size_t i;
+	efi_status_t r;
+	size_t connected = 0;
+
+	/* Get buffer with all handles with driver binding protocol */
+	r = EFI_CALL(efi_locate_handle_buffer(BY_PROTOCOL,
+					      &efi_guid_driver_binding_protocol,
+					      NULL, &count, &buffer));
+	if (r != EFI_SUCCESS)
+		return r;
+
+	/*  Context Override */
+	if (driver_image_handle) {
+		for (; *driver_image_handle; ++driver_image_handle) {
+			for (i = 0; i < count; ++i) {
+				if (buffer[i] == *driver_image_handle) {
+					buffer[i] = NULL;
+					r = efi_bind_controller(
+							controller_handle,
+							*driver_image_handle,
+							remain_device_path);
+					/*
+					 * For drivers that do not support the
+					 * controller or are already connected
+					 * we receive an error code here.
+					 */
+					if (r == EFI_SUCCESS)
+						++connected;
+				}
+			}
+		}
+	}
+
+	/*
+	 * TODO: Some overrides are not yet implemented:
+	 * - Platform Driver Override
+	 * - Driver Family Override Search
+	 * - Bus Specific Driver Override
+	 */
+
+	/* Driver Binding Search */
+	for (i = 0; i < count; ++i) {
+		if (buffer[i]) {
+			r = efi_bind_controller(controller_handle,
+						buffer[i],
+						remain_device_path);
+			if (r == EFI_SUCCESS)
+				++connected;
+		}
+	}
+
+	efi_free_pool(buffer);
+	if (!connected)
+		return EFI_NOT_FOUND;
+	return EFI_SUCCESS;
+}
+
+/*
+ * Connect a controller to a driver.
+ *
+ * This function implements the ConnectController service.
+ * See the Unified Extensible Firmware Interface (UEFI) specification
+ * for details.
+ *
+ * First all driver binding protocol handles are tried for binding drivers.
+ * Afterwards all handles that have openened a protocol of the controller
+ * with EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER are connected to drivers.
+ *
+ * @controller_handle	handle of the controller
+ * @driver_image_handle	handle of the driver
+ * @remain_device_path	device path of a child controller
+ * @recursive		true to connect all child controllers
+ * @return		status code
+ */
+static efi_status_t EFIAPI efi_connect_controller(
+			efi_handle_t controller_handle,
+			efi_handle_t *driver_image_handle,
+			struct efi_device_path *remain_device_path,
+			bool recursive)
+{
+	efi_status_t r;
+	efi_status_t ret = EFI_NOT_FOUND;
+	struct efi_object *efiobj;
+
+	EFI_ENTRY("%p, %p, %p, %d", controller_handle, driver_image_handle,
+		  remain_device_path, recursive);
+
+	efiobj = efi_search_obj(controller_handle);
+	if (!efiobj) {
+		ret = EFI_INVALID_PARAMETER;
+		goto out;
+	}
+
+	r = efi_connect_single_controller(controller_handle,
+					  driver_image_handle,
+					  remain_device_path);
+	if (r == EFI_SUCCESS)
+		ret = EFI_SUCCESS;
+	if (recursive) {
+		struct efi_handler *handler;
+		struct efi_open_protocol_info_item *item;
+
+		list_for_each_entry(handler, &efiobj->protocols, link) {
+			list_for_each_entry(item, &handler->open_infos, link) {
+				if (item->info.attributes &
+				    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER) {
+					r = EFI_CALL(efi_connect_controller(
+						item->info.controller_handle,
+						driver_image_handle,
+						remain_device_path,
+						recursive));
+					if (r == EFI_SUCCESS)
+						ret = EFI_SUCCESS;
+				}
+			}
+		}
+	}
+	/*  Check for child controller specified by end node */
+	if (ret != EFI_SUCCESS && remain_device_path &&
+	    remain_device_path->type == DEVICE_PATH_TYPE_END)
+		ret = EFI_SUCCESS;
+out:
+	return EFI_EXIT(ret);
+}
+
+/*
+ * Get all child controllers associated to a driver.
+ * The allocated buffer has to be freed with free().
+ *
+ * @efiobj			handle of the controller
+ * @driver_handle		handle of the driver
+ * @number_of_children		number of child controllers
+ * @child_handle_buffer		handles of the the child controllers
+ */
+static efi_status_t efi_get_child_controllers(
+				struct efi_object *efiobj,
+				efi_handle_t driver_handle,
+				efi_uintn_t *number_of_children,
+				efi_handle_t **child_handle_buffer)
+{
+	struct efi_handler *handler;
+	struct efi_open_protocol_info_item *item;
+	efi_uintn_t count = 0, i;
+	bool duplicate;
+
+	/* Count all child controller associations */
+	list_for_each_entry(handler, &efiobj->protocols, link) {
+		list_for_each_entry(item, &handler->open_infos, link) {
+			if (item->info.agent_handle == driver_handle &&
+			    item->info.attributes &
+			    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER)
+				++count;
+		}
+	}
+	/*
+	 * Create buffer. In case of duplicate child controller assignments
+	 * the buffer will be too large. But that does not harm.
+	 */
+	*number_of_children = 0;
+	*child_handle_buffer = calloc(count, sizeof(efi_handle_t));
+	if (!*child_handle_buffer)
+		return EFI_OUT_OF_RESOURCES;
+	/* Copy unique child handles */
+	list_for_each_entry(handler, &efiobj->protocols, link) {
+		list_for_each_entry(item, &handler->open_infos, link) {
+			if (item->info.agent_handle == driver_handle &&
+			    item->info.attributes &
+			    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER) {
+				/* Check this is a new child controller */
+				duplicate = false;
+				for (i = 0; i < *number_of_children; ++i) {
+					if ((*child_handle_buffer)[i] ==
+					    item->info.controller_handle)
+						duplicate = true;
+				}
+				/* Copy handle to buffer */
+				if (!duplicate) {
+					i = (*number_of_children)++;
+					(*child_handle_buffer)[i] =
+						item->info.controller_handle;
+				}
+			}
+		}
+	}
+	return EFI_SUCCESS;
+}
+
+/*
+ * Disconnect a controller from a driver.
+ *
+ * This function implements the DisconnectController service.
+ * See the Unified Extensible Firmware Interface (UEFI) specification
+ * for details.
+ *
+ * @controller_handle	handle of the controller
+ * @driver_image_handle handle of the driver
+ * @child_handle	handle of the child to destroy
+ * @return		status code
+ */
+static efi_status_t EFIAPI efi_disconnect_controller(
+				efi_handle_t controller_handle,
+				efi_handle_t driver_image_handle,
+				efi_handle_t child_handle)
+{
+	struct efi_driver_binding_protocol *binding_protocol;
+	efi_handle_t *child_handle_buffer = NULL;
+	size_t number_of_children = 0;
+	efi_status_t r;
+	size_t stop_count = 0;
+	struct efi_object *efiobj;
+
+	EFI_ENTRY("%p, %p, %p", controller_handle, driver_image_handle,
+		  child_handle);
+
+	efiobj = efi_search_obj(controller_handle);
+	if (!efiobj) {
+		r = EFI_INVALID_PARAMETER;
+		goto out;
+	}
+
+	if (child_handle && !efi_search_obj(child_handle)) {
+		r = EFI_INVALID_PARAMETER;
+		goto out;
+	}
+
+	/* If no driver handle is supplied, disconnect all drivers */
+	if (!driver_image_handle) {
+		r = efi_disconnect_all_drivers(efiobj, NULL, child_handle);
+		goto out;
+	}
+
+	/* Create list of child handles */
+	if (child_handle) {
+		number_of_children = 1;
+		child_handle_buffer = &child_handle;
+	} else {
+		efi_get_child_controllers(efiobj,
+					  driver_image_handle,
+					  &number_of_children,
+					  &child_handle_buffer);
+	}
+
+	/* Get the driver binding protocol */
+	r = EFI_CALL(efi_open_protocol(driver_image_handle,
+				       &efi_guid_driver_binding_protocol,
+				       (void **)&binding_protocol,
+				       driver_image_handle, NULL,
+				       EFI_OPEN_PROTOCOL_GET_PROTOCOL));
+	if (r != EFI_SUCCESS)
+		goto out;
+	/* Remove the children */
+	if (number_of_children) {
+		r = EFI_CALL(binding_protocol->stop(binding_protocol,
+						    controller_handle,
+						    number_of_children,
+						    child_handle_buffer));
+		if (r == EFI_SUCCESS)
+			++stop_count;
+	}
+	/* Remove the driver */
+	if (!child_handle)
+		r = EFI_CALL(binding_protocol->stop(binding_protocol,
+						    controller_handle,
+						    0, NULL));
+	if (r == EFI_SUCCESS)
+		++stop_count;
+	EFI_CALL(efi_close_protocol(driver_image_handle,
+				    &efi_guid_driver_binding_protocol,
+				    driver_image_handle, NULL));
+
+	if (stop_count)
+		r = EFI_SUCCESS;
+	else
+		r = EFI_NOT_FOUND;
+out:
+	if (!child_handle)
+		free(child_handle_buffer);
+	return EFI_EXIT(r);
+}
+
 static const struct efi_boot_services efi_boot_services = {
 	.hdr = {
 		.headersize = sizeof(struct efi_table_hdr),
@@ -2201,8 +2844,7 @@ static const struct efi_boot_services efi_boot_services = {
 };
 
 
-static uint16_t __efi_runtime_data firmware_vendor[] =
-	{ 'D','a','s',' ','U','-','b','o','o','t',0 };
+static uint16_t __efi_runtime_data firmware_vendor[] = L"Das U-Boot";
 
 struct efi_system_table __efi_runtime_data systab = {
 	.hdr = {
diff --git a/lib/efi_loader/efi_console.c b/lib/efi_loader/efi_console.c
index 98497db612..28d63635ec 100644
--- a/lib/efi_loader/efi_console.c
+++ b/lib/efi_loader/efi_console.c
@@ -482,18 +482,26 @@ static void EFIAPI efi_key_notify(struct efi_event *event, void *context)
 {
 }
 
+/*
+ * Notification function of the console timer event.
+ *
+ * event:	console timer event
+ * context:	not used
+ */
 static void EFIAPI efi_console_timer_notify(struct efi_event *event,
 					    void *context)
 {
 	EFI_ENTRY("%p, %p", event, context);
+
+	/* Check if input is available */
 	if (tstc()) {
+		/* Queue the wait for key event */
 		efi_con_in.wait_for_key->is_signaled = true;
-		efi_signal_event(efi_con_in.wait_for_key);
-		}
+		efi_signal_event(efi_con_in.wait_for_key, true);
+	}
 	EFI_EXIT(EFI_SUCCESS);
 }
 
-
 /* This gets called from do_bootefi_exec(). */
 int efi_console_register(void)
 {
@@ -503,21 +511,21 @@ int efi_console_register(void)
 	struct efi_object *efi_console_input_obj;
 
 	/* Create handles */
-	r = efi_create_handle((void **)&efi_console_control_obj);
+	r = efi_create_handle((efi_handle_t *)&efi_console_control_obj);
 	if (r != EFI_SUCCESS)
 		goto out_of_memory;
 	r = efi_add_protocol(efi_console_control_obj->handle,
 			     &efi_guid_console_control, &efi_console_control);
 	if (r != EFI_SUCCESS)
 		goto out_of_memory;
-	r = efi_create_handle((void **)&efi_console_output_obj);
+	r = efi_create_handle((efi_handle_t *)&efi_console_output_obj);
 	if (r != EFI_SUCCESS)
 		goto out_of_memory;
 	r = efi_add_protocol(efi_console_output_obj->handle,
 			     &efi_guid_text_output_protocol, &efi_con_out);
 	if (r != EFI_SUCCESS)
 		goto out_of_memory;
-	r = efi_create_handle((void **)&efi_console_input_obj);
+	r = efi_create_handle((efi_handle_t *)&efi_console_input_obj);
 	if (r != EFI_SUCCESS)
 		goto out_of_memory;
 	r = efi_add_protocol(efi_console_input_obj->handle,
diff --git a/lib/efi_loader/efi_device_path.c b/lib/efi_loader/efi_device_path.c
index ccb59337f1..ecc4eda3f8 100644
--- a/lib/efi_loader/efi_device_path.c
+++ b/lib/efi_loader/efi_device_path.c
@@ -6,6 +6,8 @@
  * SPDX-License-Identifier:	GPL-2.0+
  */
 
+#define LOG_CATEGORY LOGL_ERR
+
 #include <common.h>
 #include <blk.h>
 #include <dm.h>
@@ -58,8 +60,11 @@ static void *dp_alloc(size_t sz)
 {
 	void *buf;
 
-	if (efi_allocate_pool(EFI_ALLOCATE_ANY_PAGES, sz, &buf) != EFI_SUCCESS)
+	if (efi_allocate_pool(EFI_ALLOCATE_ANY_PAGES, sz, &buf) !=
+	    EFI_SUCCESS) {
+		debug("EFI: ERROR: out of memory in %s\n", __func__);
 		return NULL;
+	}
 
 	return buf;
 }
@@ -108,7 +113,6 @@ int efi_dp_match(const struct efi_device_path *a,
 	}
 }
 
-
 /*
  * See UEFI spec (section 3.1.2, about short-form device-paths..
  * tl;dr: we can have a device-path that starts with a USB WWID
@@ -181,7 +185,6 @@ static struct efi_object *find_obj(struct efi_device_path *dp, bool short_path,
 	return NULL;
 }
 
-
 /*
  * Find an efiobj from device-path, if 'rem' is not NULL, returns the
  * remaining part of the device path after the matched object.
@@ -205,6 +208,26 @@ struct efi_object *efi_dp_find_obj(struct efi_device_path *dp,
 	return efiobj;
 }
 
+/*
+ * Determine the last device path node that is not the end node.
+ *
+ * @dp		device path
+ * @return	last node before the end node if it exists
+ *		otherwise NULL
+ */
+const struct efi_device_path *efi_dp_last_node(const struct efi_device_path *dp)
+{
+	struct efi_device_path *ret;
+
+	if (!dp || dp->type == DEVICE_PATH_TYPE_END)
+		return NULL;
+	while (dp) {
+		ret = (struct efi_device_path *)dp;
+		dp = efi_dp_next(dp);
+	}
+	return ret;
+}
+
 /* return size not including End node: */
 unsigned efi_dp_size(const struct efi_device_path *dp)
 {
@@ -227,6 +250,8 @@ struct efi_device_path *efi_dp_dup(const struct efi_device_path *dp)
 		return NULL;
 
 	ndp = dp_alloc(sz);
+	if (!ndp)
+		return NULL;
 	memcpy(ndp, dp, sz);
 
 	return ndp;
@@ -246,6 +271,8 @@ struct efi_device_path *efi_dp_append(const struct efi_device_path *dp1,
 		unsigned sz1 = efi_dp_size(dp1);
 		unsigned sz2 = efi_dp_size(dp2);
 		void *p = dp_alloc(sz1 + sz2 + sizeof(END));
+		if (!p)
+			return NULL;
 		memcpy(p, dp1, sz1);
 		memcpy(p + sz1, dp2, sz2);
 		memcpy(p + sz1 + sz2, &END, sizeof(END));
@@ -267,6 +294,8 @@ struct efi_device_path *efi_dp_append_node(const struct efi_device_path *dp,
 	} else if (!dp) {
 		unsigned sz = node->length;
 		void *p = dp_alloc(sz + sizeof(END));
+		if (!p)
+			return NULL;
 		memcpy(p, node, sz);
 		memcpy(p + sz, &END, sizeof(END));
 		ret = p;
@@ -274,6 +303,8 @@ struct efi_device_path *efi_dp_append_node(const struct efi_device_path *dp,
 		/* both dp and node are non-null */
 		unsigned sz = efi_dp_size(dp);
 		void *p = dp_alloc(sz + node->length + sizeof(END));
+		if (!p)
+			return NULL;
 		memcpy(p, dp, sz);
 		memcpy(p + sz, node, node->length);
 		memcpy(p + sz + node->length, &END, sizeof(END));
@@ -297,9 +328,36 @@ static unsigned dp_size(struct udevice *dev)
 	case UCLASS_SIMPLE_BUS:
 		/* stop traversing parents at this point: */
 		return sizeof(ROOT);
+	case UCLASS_ETH:
+		return dp_size(dev->parent) +
+			sizeof(struct efi_device_path_mac_addr);
+#ifdef CONFIG_BLK
+	case UCLASS_BLK:
+		switch (dev->parent->uclass->uc_drv->id) {
+#ifdef CONFIG_IDE
+		case UCLASS_IDE:
+			return dp_size(dev->parent) +
+				sizeof(struct efi_device_path_atapi);
+#endif
+#if defined(CONFIG_SCSI) && defined(CONFIG_DM_SCSI)
+		case UCLASS_SCSI:
+			return dp_size(dev->parent) +
+				sizeof(struct efi_device_path_scsi);
+#endif
+#if defined(CONFIG_DM_MMC) && defined(CONFIG_MMC)
+		case UCLASS_MMC:
+			return dp_size(dev->parent) +
+				sizeof(struct efi_device_path_sd_mmc_path);
+#endif
+		default:
+			return dp_size(dev->parent);
+		}
+#endif
+#if defined(CONFIG_DM_MMC) && defined(CONFIG_MMC)
 	case UCLASS_MMC:
 		return dp_size(dev->parent) +
 			sizeof(struct efi_device_path_sd_mmc_path);
+#endif
 	case UCLASS_MASS_STORAGE:
 	case UCLASS_USB_HUB:
 		return dp_size(dev->parent) +
@@ -310,6 +368,13 @@ static unsigned dp_size(struct udevice *dev)
 	}
 }
 
+/*
+ * Recursively build a device path.
+ *
+ * @buf		pointer to the end of the device path
+ * @dev		device
+ * @return	pointer to the end of the device path
+ */
 static void *dp_fill(void *buf, struct udevice *dev)
 {
 	if (!dev || !dev->driver)
@@ -323,6 +388,79 @@ static void *dp_fill(void *buf, struct udevice *dev)
 		*vdp = ROOT;
 		return &vdp[1];
 	}
+#ifdef CONFIG_DM_ETH
+	case UCLASS_ETH: {
+		struct efi_device_path_mac_addr *dp =
+			dp_fill(buf, dev->parent);
+		struct eth_pdata *pdata = dev->platdata;
+
+		dp->dp.type = DEVICE_PATH_TYPE_MESSAGING_DEVICE;
+		dp->dp.sub_type = DEVICE_PATH_SUB_TYPE_MSG_MAC_ADDR;
+		dp->dp.length = sizeof(*dp);
+		memset(&dp->mac, 0, sizeof(dp->mac));
+		/* We only support IPv4 */
+		memcpy(&dp->mac, &pdata->enetaddr, ARP_HLEN);
+		/* Ethernet */
+		dp->if_type = 1;
+		return &dp[1];
+	}
+#endif
+#ifdef CONFIG_BLK
+	case UCLASS_BLK:
+		switch (dev->parent->uclass->uc_drv->id) {
+#ifdef CONFIG_IDE
+		case UCLASS_IDE: {
+			struct efi_device_path_atapi *dp =
+			dp_fill(buf, dev->parent);
+			struct blk_desc *desc = dev_get_uclass_platdata(dev);
+
+			dp->dp.type = DEVICE_PATH_TYPE_MESSAGING_DEVICE;
+			dp->dp.sub_type = DEVICE_PATH_SUB_TYPE_MSG_ATAPI;
+			dp->dp.length = sizeof(*dp);
+			dp->logical_unit_number = desc->devnum;
+			dp->primary_secondary = IDE_BUS(desc->devnum);
+			dp->slave_master = desc->devnum %
+				(CONFIG_SYS_IDE_MAXDEVICE /
+				 CONFIG_SYS_IDE_MAXBUS);
+			return &dp[1];
+			}
+#endif
+#if defined(CONFIG_SCSI) && defined(CONFIG_DM_SCSI)
+		case UCLASS_SCSI: {
+			struct efi_device_path_scsi *dp =
+				dp_fill(buf, dev->parent);
+			struct blk_desc *desc = dev_get_uclass_platdata(dev);
+
+			dp->dp.type = DEVICE_PATH_TYPE_MESSAGING_DEVICE;
+			dp->dp.sub_type = DEVICE_PATH_SUB_TYPE_MSG_SCSI;
+			dp->dp.length = sizeof(*dp);
+			dp->logical_unit_number = desc->lun;
+			dp->target_id = desc->target;
+			return &dp[1];
+			}
+#endif
+#if defined(CONFIG_DM_MMC) && defined(CONFIG_MMC)
+		case UCLASS_MMC: {
+			struct efi_device_path_sd_mmc_path *sddp =
+				dp_fill(buf, dev->parent);
+			struct blk_desc *desc = dev_get_uclass_platdata(dev);
+
+			sddp->dp.type     = DEVICE_PATH_TYPE_MESSAGING_DEVICE;
+			sddp->dp.sub_type = is_sd(desc) ?
+				DEVICE_PATH_SUB_TYPE_MSG_SD :
+				DEVICE_PATH_SUB_TYPE_MSG_MMC;
+			sddp->dp.length   = sizeof(*sddp);
+			sddp->slot_number = dev->seq;
+			return &sddp[1];
+			}
+#endif
+		default:
+			debug("%s(%u) %s: unhandled parent class: %s (%u)\n",
+			      __FILE__, __LINE__, __func__,
+			      dev->name, dev->parent->uclass->uc_drv->id);
+			return dp_fill(buf, dev->parent);
+		}
+#endif
 #if defined(CONFIG_DM_MMC) && defined(CONFIG_MMC)
 	case UCLASS_MMC: {
 		struct efi_device_path_sd_mmc_path *sddp =
@@ -359,7 +497,8 @@ static void *dp_fill(void *buf, struct udevice *dev)
 		return &udp[1];
 	}
 	default:
-		debug("unhandled device class: %s (%u)\n",
+		debug("%s(%u) %s: unhandled device class: %s (%u)\n",
+		      __FILE__, __LINE__, __func__,
 		      dev->name, dev->driver->id);
 		return dp_fill(buf, dev->parent);
 	}
@@ -371,6 +510,8 @@ struct efi_device_path *efi_dp_from_dev(struct udevice *dev)
 	void *buf, *start;
 
 	start = buf = dp_alloc(dp_size(dev) + sizeof(END));
+	if (!buf)
+		return NULL;
 	buf = dp_fill(buf, dev);
 	*((struct efi_device_path *)buf) = END;
 
@@ -383,7 +524,14 @@ static unsigned dp_part_size(struct blk_desc *desc, int part)
 	unsigned dpsize;
 
 #ifdef CONFIG_BLK
-	dpsize = dp_size(desc->bdev->parent);
+	{
+		struct udevice *dev;
+		int ret = blk_find_device(desc->if_type, desc->devnum, &dev);
+
+		if (ret)
+			dev = desc->bdev->parent;
+		dpsize = dp_size(dev);
+	}
 #else
 	dpsize = sizeof(ROOT) + sizeof(struct efi_device_path_usb);
 #endif
@@ -400,43 +548,16 @@ static unsigned dp_part_size(struct blk_desc *desc, int part)
 }
 
 /*
- * Create a device path for a block device or one of its partitions.
+ * Create a device node for a block device partition.
  *
  * @buf		buffer to which the device path is wirtten
  * @desc	block device descriptor
  * @part	partition number, 0 identifies a block device
  */
-static void *dp_part_fill(void *buf, struct blk_desc *desc, int part)
+static void *dp_part_node(void *buf, struct blk_desc *desc, int part)
 {
 	disk_partition_t info;
 
-#ifdef CONFIG_BLK
-	buf = dp_fill(buf, desc->bdev->parent);
-#else
-	/*
-	 * We *could* make a more accurate path, by looking at if_type
-	 * and handling all the different cases like we do for non-
-	 * legacy (ie CONFIG_BLK=y) case.  But most important thing
-	 * is just to have a unique device-path for if_type+devnum.
-	 * So map things to a fictitious USB device.
-	 */
-	struct efi_device_path_usb *udp;
-
-	memcpy(buf, &ROOT, sizeof(ROOT));
-	buf += sizeof(ROOT);
-
-	udp = buf;
-	udp->dp.type = DEVICE_PATH_TYPE_MESSAGING_DEVICE;
-	udp->dp.sub_type = DEVICE_PATH_SUB_TYPE_MSG_USB;
-	udp->dp.length = sizeof(*udp);
-	udp->parent_port_number = desc->if_type;
-	udp->usb_interface = desc->devnum;
-	buf = &udp[1];
-#endif
-
-	if (part == 0) /* the actual disk, not a partition */
-		return buf;
-
 	part_get_info(desc, part, &info);
 
 	if (desc->part_type == PART_TYPE_ISO) {
@@ -491,6 +612,51 @@ static void *dp_part_fill(void *buf, struct blk_desc *desc, int part)
 	return buf;
 }
 
+/*
+ * Create a device path for a block device or one of its partitions.
+ *
+ * @buf		buffer to which the device path is wirtten
+ * @desc	block device descriptor
+ * @part	partition number, 0 identifies a block device
+ */
+static void *dp_part_fill(void *buf, struct blk_desc *desc, int part)
+{
+#ifdef CONFIG_BLK
+	{
+		struct udevice *dev;
+		int ret = blk_find_device(desc->if_type, desc->devnum, &dev);
+
+		if (ret)
+			dev = desc->bdev->parent;
+		buf = dp_fill(buf, dev);
+	}
+#else
+	/*
+	 * We *could* make a more accurate path, by looking at if_type
+	 * and handling all the different cases like we do for non-
+	 * legacy (ie CONFIG_BLK=y) case.  But most important thing
+	 * is just to have a unique device-path for if_type+devnum.
+	 * So map things to a fictitious USB device.
+	 */
+	struct efi_device_path_usb *udp;
+
+	memcpy(buf, &ROOT, sizeof(ROOT));
+	buf += sizeof(ROOT);
+
+	udp = buf;
+	udp->dp.type = DEVICE_PATH_TYPE_MESSAGING_DEVICE;
+	udp->dp.sub_type = DEVICE_PATH_SUB_TYPE_MSG_USB;
+	udp->dp.length = sizeof(*udp);
+	udp->parent_port_number = desc->if_type;
+	udp->usb_interface = desc->devnum;
+	buf = &udp[1];
+#endif
+
+	if (part == 0) /* the actual disk, not a partition */
+		return buf;
+
+	return dp_part_node(buf, desc, part);
+}
 
 /* Construct a device-path from a partition on a blk device: */
 struct efi_device_path *efi_dp_from_part(struct blk_desc *desc, int part)
@@ -498,6 +664,8 @@ struct efi_device_path *efi_dp_from_part(struct blk_desc *desc, int part)
 	void *buf, *start;
 
 	start = buf = dp_alloc(dp_part_size(desc, part) + sizeof(END));
+	if (!buf)
+		return NULL;
 
 	buf = dp_part_fill(buf, desc, part);
 
@@ -506,6 +674,29 @@ struct efi_device_path *efi_dp_from_part(struct blk_desc *desc, int part)
 	return start;
 }
 
+/*
+ * Create a device node for a block device partition.
+ *
+ * @buf		buffer to which the device path is wirtten
+ * @desc	block device descriptor
+ * @part	partition number, 0 identifies a block device
+ */
+struct efi_device_path *efi_dp_part_node(struct blk_desc *desc, int part)
+{
+	efi_uintn_t dpsize;
+	void *buf;
+
+	if (desc->part_type == PART_TYPE_ISO)
+		dpsize = sizeof(struct efi_device_path_cdrom_path);
+	else
+		dpsize = sizeof(struct efi_device_path_hard_drive_path);
+	buf = dp_alloc(dpsize);
+
+	dp_part_node(buf, desc, part);
+
+	return buf;
+}
+
 /* convert path to an UEFI style path (ie. DOS style backslashes and utf16) */
 static void path_to_uefi(u16 *uefi, const char *path)
 {
@@ -536,6 +727,8 @@ struct efi_device_path *efi_dp_from_file(struct blk_desc *desc, int part,
 	dpsize += fpsize;
 
 	start = buf = dp_alloc(dpsize + sizeof(END));
+	if (!buf)
+		return NULL;
 
 	if (desc)
 		buf = dp_part_fill(buf, desc, part);
@@ -570,6 +763,8 @@ struct efi_device_path *efi_dp_from_eth(void)
 	dpsize += sizeof(*ndp);
 
 	start = buf = dp_alloc(dpsize + sizeof(END));
+	if (!buf)
+		return NULL;
 
 #ifdef CONFIG_DM_ETH
 	buf = dp_fill(buf, eth_get_dev());
@@ -600,6 +795,8 @@ struct efi_device_path *efi_dp_from_mem(uint32_t memory_type,
 	void *buf, *start;
 
 	start = buf = dp_alloc(sizeof(*mdp) + sizeof(END));
+	if (!buf)
+		return NULL;
 
 	mdp = buf;
 	mdp->dp.type = DEVICE_PATH_TYPE_HARDWARE_DEVICE;
@@ -619,22 +816,31 @@ struct efi_device_path *efi_dp_from_mem(uint32_t memory_type,
  * Helper to split a full device path (containing both device and file
  * parts) into it's constituent parts.
  */
-void efi_dp_split_file_path(struct efi_device_path *full_path,
-			    struct efi_device_path **device_path,
-			    struct efi_device_path **file_path)
+efi_status_t efi_dp_split_file_path(struct efi_device_path *full_path,
+				    struct efi_device_path **device_path,
+				    struct efi_device_path **file_path)
 {
 	struct efi_device_path *p, *dp, *fp;
 
+	*device_path = NULL;
+	*file_path = NULL;
 	dp = efi_dp_dup(full_path);
+	if (!dp)
+		return EFI_OUT_OF_RESOURCES;
 	p = dp;
-	while (!EFI_DP_TYPE(p, MEDIA_DEVICE, FILE_PATH))
+	while (!EFI_DP_TYPE(p, MEDIA_DEVICE, FILE_PATH)) {
 		p = efi_dp_next(p);
+		if (!p)
+			return EFI_OUT_OF_RESOURCES;
+	}
 	fp = efi_dp_dup(p);
-
+	if (!fp)
+		return EFI_OUT_OF_RESOURCES;
 	p->type = DEVICE_PATH_TYPE_END;
 	p->sub_type = DEVICE_PATH_SUB_TYPE_END;
 	p->length = sizeof(*p);
 
 	*device_path = dp;
 	*file_path = fp;
+	return EFI_SUCCESS;
 }
diff --git a/lib/efi_loader/efi_device_path_to_text.c b/lib/efi_loader/efi_device_path_to_text.c
index 50d9e911c0..a79e60a4ee 100644
--- a/lib/efi_loader/efi_device_path_to_text.c
+++ b/lib/efi_loader/efi_device_path_to_text.c
@@ -87,6 +87,20 @@ static char *dp_acpi(char *s, struct efi_device_path *dp)
 static char *dp_msging(char *s, struct efi_device_path *dp)
 {
 	switch (dp->sub_type) {
+	case DEVICE_PATH_SUB_TYPE_MSG_ATAPI: {
+		struct efi_device_path_atapi *ide =
+			(struct efi_device_path_atapi *)dp;
+		s += sprintf(s, "Ata(%d,%d,%d)", ide->primary_secondary,
+			     ide->slave_master, ide->logical_unit_number);
+		break;
+	}
+	case DEVICE_PATH_SUB_TYPE_MSG_SCSI: {
+		struct efi_device_path_scsi *ide =
+			(struct efi_device_path_scsi *)dp;
+		s += sprintf(s, "Scsi(%u,%u)", ide->target_id,
+			     ide->logical_unit_number);
+		break;
+	}
 	case DEVICE_PATH_SUB_TYPE_MSG_USB: {
 		struct efi_device_path_usb *udp =
 			(struct efi_device_path_usb *)dp;
@@ -231,6 +245,8 @@ static char *efi_convert_single_device_node_to_text(
 	case DEVICE_PATH_TYPE_MEDIA_DEVICE:
 		str = dp_media(str, dp);
 		break;
+	case DEVICE_PATH_TYPE_END:
+		break;
 	default:
 		str = dp_unknown(str, dp);
 	}
diff --git a/lib/efi_loader/efi_disk.c b/lib/efi_loader/efi_disk.c
index d299fc8dea..ac39a65ee8 100644
--- a/lib/efi_loader/efi_disk.c
+++ b/lib/efi_loader/efi_disk.c
@@ -14,7 +14,7 @@
 #include <part.h>
 #include <malloc.h>
 
-static const efi_guid_t efi_block_io_guid = BLOCK_IO_GUID;
+const efi_guid_t efi_block_io_guid = BLOCK_IO_GUID;
 
 struct efi_disk_obj {
 	/* Generic EFI object parent class data */
@@ -91,7 +91,7 @@ static efi_status_t efi_disk_rw_blocks(struct efi_block_io *this,
 }
 
 static efi_status_t EFIAPI efi_disk_read_blocks(struct efi_block_io *this,
-			u32 media_id, u64 lba, unsigned long buffer_size,
+			u32 media_id, u64 lba, efi_uintn_t buffer_size,
 			void *buffer)
 {
 	void *real_buffer = buffer;
@@ -112,7 +112,7 @@ static efi_status_t EFIAPI efi_disk_read_blocks(struct efi_block_io *this,
 	real_buffer = efi_bounce_buffer;
 #endif
 
-	EFI_ENTRY("%p, %x, %"PRIx64", %lx, %p", this, media_id, lba,
+	EFI_ENTRY("%p, %x, %" PRIx64 ", %zx, %p", this, media_id, lba,
 		  buffer_size, buffer);
 
 	r = efi_disk_rw_blocks(this, media_id, lba, buffer_size, real_buffer,
@@ -126,7 +126,7 @@ static efi_status_t EFIAPI efi_disk_read_blocks(struct efi_block_io *this,
 }
 
 static efi_status_t EFIAPI efi_disk_write_blocks(struct efi_block_io *this,
-			u32 media_id, u64 lba, unsigned long buffer_size,
+			u32 media_id, u64 lba, efi_uintn_t buffer_size,
 			void *buffer)
 {
 	void *real_buffer = buffer;
@@ -147,7 +147,7 @@ static efi_status_t EFIAPI efi_disk_write_blocks(struct efi_block_io *this,
 	real_buffer = efi_bounce_buffer;
 #endif
 
-	EFI_ENTRY("%p, %x, %"PRIx64", %lx, %p", this, media_id, lba,
+	EFI_ENTRY("%p, %x, %" PRIx64 ", %zx, %p", this, media_id, lba,
 		  buffer_size, buffer);
 
 	/* Populate bounce buffer if necessary */
@@ -175,49 +175,72 @@ static const struct efi_block_io block_io_disk_template = {
 };
 
 /*
- * Find filesystem from a device-path.  The passed in path 'p' probably
- * contains one or more /File(name) nodes, so the comparison stops at
- * the first /File() node, and returns the pointer to that via 'rp'.
- * This is mostly intended to be a helper to map a device-path to an
- * efi_file_handle object.
+ * Get the simple file system protocol for a file device path.
+ *
+ * The full path provided is split into device part and into a file
+ * part. The device part is used to find the handle on which the
+ * simple file system protocol is installed.
+ *
+ * @full_path	device path including device and file
+ * @return	simple file system protocol
  */
 struct efi_simple_file_system_protocol *
-efi_fs_from_path(struct efi_device_path *fp)
+efi_fs_from_path(struct efi_device_path *full_path)
 {
 	struct efi_object *efiobj;
-	struct efi_disk_obj *diskobj;
+	struct efi_handler *handler;
+	struct efi_device_path *device_path;
+	struct efi_device_path *file_path;
+	efi_status_t ret;
 
-	efiobj = efi_dp_find_obj(fp, NULL);
+	/* Split the path into a device part and a file part */
+	ret = efi_dp_split_file_path(full_path, &device_path, &file_path);
+	if (ret != EFI_SUCCESS)
+		return NULL;
+	efi_free_pool(file_path);
+
+	/* Get the EFI object for the partition */
+	efiobj = efi_dp_find_obj(device_path, NULL);
+	efi_free_pool(device_path);
 	if (!efiobj)
 		return NULL;
 
-	diskobj = container_of(efiobj, struct efi_disk_obj, parent);
+	/* Find the simple file system protocol */
+	ret = efi_search_protocol(efiobj, &efi_simple_file_system_protocol_guid,
+				  &handler);
+	if (ret != EFI_SUCCESS)
+		return NULL;
 
-	return diskobj->volume;
+	/* Return the simple file system protocol for the partition */
+	return handler->protocol_interface;
 }
 
 /*
- * Create a device for a disk
+ * Create a handle for a partition or disk
  *
- * @name	not used
+ * @parent	parent handle
+ * @dp_parent	parent device path
  * @if_typename interface name for block device
  * @desc	internal block device
  * @dev_index   device index for block device
  * @offset	offset into disk for simple partitions
+ * @return	disk object
  */
-static void efi_disk_add_dev(const char *name,
-			     const char *if_typename,
-			     struct blk_desc *desc,
-			     int dev_index,
-			     lbaint_t offset,
-			     unsigned int part)
+static struct efi_disk_obj *efi_disk_add_dev(
+				efi_handle_t parent,
+				struct efi_device_path *dp_parent,
+				const char *if_typename,
+				struct blk_desc *desc,
+				int dev_index,
+				lbaint_t offset,
+				unsigned int part)
 {
 	struct efi_disk_obj *diskobj;
 	efi_status_t ret;
 
 	/* Don't add empty devices */
 	if (!desc->lba)
-		return;
+		return NULL;
 
 	diskobj = calloc(1, sizeof(*diskobj));
 	if (!diskobj)
@@ -227,7 +250,14 @@ static void efi_disk_add_dev(const char *name,
 	efi_add_handle(&diskobj->parent);
 
 	/* Fill in object data */
-	diskobj->dp = efi_dp_from_part(desc, part);
+	if (part) {
+		struct efi_device_path *node = efi_dp_part_node(desc, part);
+
+		diskobj->dp = efi_dp_append_node(dp_parent, node);
+		efi_free_pool(node);
+	} else {
+		diskobj->dp = efi_dp_from_part(desc, part);
+	}
 	diskobj->part = part;
 	ret = efi_add_protocol(diskobj->parent.handle, &efi_block_io_guid,
 			       &diskobj->ops);
@@ -242,7 +272,7 @@ static void efi_disk_add_dev(const char *name,
 							 diskobj->dp);
 		ret = efi_add_protocol(diskobj->parent.handle,
 				       &efi_simple_file_system_protocol_guid,
-				       &diskobj->volume);
+				       diskobj->volume);
 		if (ret != EFI_SUCCESS)
 			goto out_of_memory;
 	}
@@ -261,20 +291,38 @@ static void efi_disk_add_dev(const char *name,
 	if (part != 0)
 		diskobj->media.logical_partition = 1;
 	diskobj->ops.media = &diskobj->media;
-	return;
+	return diskobj;
 out_of_memory:
 	printf("ERROR: Out of memory\n");
+	return NULL;
 }
 
-static int efi_disk_create_partitions(struct blk_desc *desc,
-				      const char *if_typename,
-				      int diskid,
-				      const char *pdevname)
+/*
+ * Create handles and protocols for the partitions of a block device
+ *
+ * @parent		handle of the parent disk
+ * @blk_desc		block device
+ * @if_typename		interface type
+ * @diskid		device number
+ * @pdevname		device name
+ * @return		number of partitions created
+ */
+int efi_disk_create_partitions(efi_handle_t parent, struct blk_desc *desc,
+			       const char *if_typename, int diskid,
+			       const char *pdevname)
 {
 	int disks = 0;
 	char devname[32] = { 0 }; /* dp->str is u16[32] long */
 	disk_partition_t info;
 	int part;
+	struct efi_device_path *dp = NULL;
+	efi_status_t ret;
+	struct efi_handler *handler;
+
+	/* Get the device path of the parent */
+	ret = efi_search_protocol(parent, &efi_guid_device_path, &handler);
+	if (ret == EFI_SUCCESS)
+		dp = handler->protocol_interface;
 
 	/* Add devices for each partition */
 	for (part = 1; part <= MAX_SEARCH_PARTITIONS; part++) {
@@ -282,7 +330,7 @@ static int efi_disk_create_partitions(struct blk_desc *desc,
 			continue;
 		snprintf(devname, sizeof(devname), "%s:%d", pdevname,
 			 part);
-		efi_disk_add_dev(devname, if_typename, desc, diskid,
+		efi_disk_add_dev(parent, dp, if_typename, desc, diskid,
 				 info.start, part);
 		disks++;
 	}
@@ -303,6 +351,7 @@ static int efi_disk_create_partitions(struct blk_desc *desc,
  */
 int efi_disk_register(void)
 {
+	struct efi_disk_obj *disk;
 	int disks = 0;
 #ifdef CONFIG_BLK
 	struct udevice *dev;
@@ -311,19 +360,21 @@ int efi_disk_register(void)
 	     dev;
 	     uclass_next_device_check(&dev)) {
 		struct blk_desc *desc = dev_get_uclass_platdata(dev);
-		const char *if_typename = dev->driver->name;
+		const char *if_typename = blk_get_if_type_name(desc->if_type);
 
 		printf("Scanning disk %s...\n", dev->name);
 
 		/* Add block device for the full device */
-		efi_disk_add_dev(dev->name, if_typename, desc,
-				 desc->devnum, 0, 0);
-
+		disk = efi_disk_add_dev(NULL, NULL, if_typename,
+					desc, desc->devnum, 0, 0);
+		if (!disk)
+			return -ENOMEM;
 		disks++;
 
 		/* Partitions show up as block devices in EFI */
-		disks += efi_disk_create_partitions(desc, if_typename,
-						    desc->devnum, dev->name);
+		disks += efi_disk_create_partitions(
+					disk->parent.handle, desc, if_typename,
+					desc->devnum, dev->name);
 	}
 #else
 	int i, if_type;
@@ -353,12 +404,16 @@ int efi_disk_register(void)
 				 if_typename, i);
 
 			/* Add block device for the full device */
-			efi_disk_add_dev(devname, if_typename, desc, i, 0, 0);
+			disk = efi_disk_add_dev(NULL, NULL, if_typename, desc,
+						i, 0, 0);
+			if (!disk)
+				return -ENOMEM;
 			disks++;
 
 			/* Partitions show up as block devices in EFI */
-			disks += efi_disk_create_partitions(desc, if_typename,
-							    i, devname);
+			disks += efi_disk_create_partitions(
+						disk->parent.handle, desc,
+						if_typename, i, devname);
 		}
 	}
 #endif
diff --git a/lib/efi_loader/efi_image_loader.c b/lib/efi_loader/efi_image_loader.c
index af29cc4f04..9d2214b481 100644
--- a/lib/efi_loader/efi_image_loader.c
+++ b/lib/efi_loader/efi_image_loader.c
@@ -74,6 +74,40 @@ void __weak invalidate_icache_all(void)
 }
 
 /*
+ * Determine the memory types to be used for code and data.
+ *
+ * @loaded_image_info	image descriptor
+ * @image_type		field Subsystem of the optional header for
+ *			Windows specific field
+ */
+static void efi_set_code_and_data_type(
+			struct efi_loaded_image *loaded_image_info,
+			uint16_t image_type)
+{
+	switch (image_type) {
+	case IMAGE_SUBSYSTEM_EFI_APPLICATION:
+		loaded_image_info->image_code_type = EFI_LOADER_CODE;
+		loaded_image_info->image_data_type = EFI_LOADER_DATA;
+		break;
+	case IMAGE_SUBSYSTEM_EFI_BOOT_SERVICE_DRIVER:
+		loaded_image_info->image_code_type = EFI_BOOT_SERVICES_CODE;
+		loaded_image_info->image_data_type = EFI_BOOT_SERVICES_DATA;
+		break;
+	case IMAGE_SUBSYSTEM_EFI_RUNTIME_DRIVER:
+	case IMAGE_SUBSYSTEM_SAL_RUNTIME_DRIVER:
+		loaded_image_info->image_code_type = EFI_RUNTIME_SERVICES_CODE;
+		loaded_image_info->image_data_type = EFI_RUNTIME_SERVICES_DATA;
+		break;
+	default:
+		printf("%s: invalid image type: %u\n", __func__, image_type);
+		/* Let's assume it is an application */
+		loaded_image_info->image_code_type = EFI_LOADER_CODE;
+		loaded_image_info->image_data_type = EFI_LOADER_DATA;
+		break;
+	}
+}
+
+/*
  * This function loads all sections from a PE binary into a newly reserved
  * piece of memory. On successful load it then returns the entry point for
  * the binary. Otherwise NULL.
@@ -94,7 +128,6 @@ void *efi_load_pe(void *efi, struct efi_loaded_image *loaded_image_info)
 	unsigned long virt_size = 0;
 	bool can_run_nt64 = true;
 	bool can_run_nt32 = true;
-	uint16_t image_type;
 
 #if defined(CONFIG_ARM64)
 	can_run_nt32 = false;
@@ -131,55 +164,38 @@ void *efi_load_pe(void *efi, struct efi_loaded_image *loaded_image_info)
 		IMAGE_NT_HEADERS64 *nt64 = (void *)nt;
 		IMAGE_OPTIONAL_HEADER64 *opt = &nt64->OptionalHeader;
 		image_size = opt->SizeOfImage;
-		efi_reloc = efi_alloc(virt_size, EFI_LOADER_DATA);
+		efi_set_code_and_data_type(loaded_image_info, opt->Subsystem);
+		efi_reloc = efi_alloc(virt_size,
+				      loaded_image_info->image_code_type);
 		if (!efi_reloc) {
-			printf("%s: Could not allocate %ld bytes\n",
-				__func__, virt_size);
+			printf("%s: Could not allocate %lu bytes\n",
+			       __func__, virt_size);
 			return NULL;
 		}
 		entry = efi_reloc + opt->AddressOfEntryPoint;
 		rel_size = opt->DataDirectory[rel_idx].Size;
 		rel = efi_reloc + opt->DataDirectory[rel_idx].VirtualAddress;
-		image_type = opt->Subsystem;
 	} else if (can_run_nt32 &&
 		   (nt->OptionalHeader.Magic == IMAGE_NT_OPTIONAL_HDR32_MAGIC)) {
 		IMAGE_OPTIONAL_HEADER32 *opt = &nt->OptionalHeader;
 		image_size = opt->SizeOfImage;
-		efi_reloc = efi_alloc(virt_size, EFI_LOADER_DATA);
+		efi_set_code_and_data_type(loaded_image_info, opt->Subsystem);
+		efi_reloc = efi_alloc(virt_size,
+				      loaded_image_info->image_code_type);
 		if (!efi_reloc) {
-			printf("%s: Could not allocate %ld bytes\n",
-				__func__, virt_size);
+			printf("%s: Could not allocate %lu bytes\n",
+			       __func__, virt_size);
 			return NULL;
 		}
 		entry = efi_reloc + opt->AddressOfEntryPoint;
 		rel_size = opt->DataDirectory[rel_idx].Size;
 		rel = efi_reloc + opt->DataDirectory[rel_idx].VirtualAddress;
-		image_type = opt->Subsystem;
 	} else {
 		printf("%s: Invalid optional header magic %x\n", __func__,
 		       nt->OptionalHeader.Magic);
 		return NULL;
 	}
 
-	switch (image_type) {
-	case IMAGE_SUBSYSTEM_EFI_APPLICATION:
-		loaded_image_info->image_code_type = EFI_LOADER_CODE;
-		loaded_image_info->image_data_type = EFI_LOADER_DATA;
-		break;
-	case IMAGE_SUBSYSTEM_EFI_BOOT_SERVICE_DRIVER:
-		loaded_image_info->image_code_type = EFI_BOOT_SERVICES_CODE;
-		loaded_image_info->image_data_type = EFI_BOOT_SERVICES_DATA;
-		break;
-	case IMAGE_SUBSYSTEM_EFI_RUNTIME_DRIVER:
-	case IMAGE_SUBSYSTEM_SAL_RUNTIME_DRIVER:
-		loaded_image_info->image_code_type = EFI_RUNTIME_SERVICES_CODE;
-		loaded_image_info->image_data_type = EFI_RUNTIME_SERVICES_DATA;
-		break;
-	default:
-		printf("%s: invalid image type: %u\n", __func__, image_type);
-		break;
-	}
-
 	/* Load sections into RAM */
 	for (i = num_sections - 1; i >= 0; i--) {
 		IMAGE_SECTION_HEADER *sec = &sections[i];
diff --git a/lib/efi_loader/efi_memory.c b/lib/efi_loader/efi_memory.c
index 0aa3e0881d..aaf64421a3 100644
--- a/lib/efi_loader/efi_memory.c
+++ b/lib/efi_loader/efi_memory.c
@@ -275,6 +275,15 @@ static uint64_t efi_find_free_memory(uint64_t len, uint64_t max_addr)
 	return 0;
 }
 
+/*
+ * Allocate memory pages.
+ *
+ * @type		type of allocation to be performed
+ * @memory_type		usage type of the allocated memory
+ * @pages		number of pages to be allocated
+ * @memory		allocated memory
+ * @return		status code
+ */
 efi_status_t efi_allocate_pages(int type, int memory_type,
 				efi_uintn_t pages, uint64_t *memory)
 {
@@ -338,6 +347,13 @@ void *efi_alloc(uint64_t len, int memory_type)
 	return NULL;
 }
 
+/*
+ * Free memory pages.
+ *
+ * @memory	start of the memory area to be freed
+ * @pages	number of pages to be freed
+ * @return	status code
+ */
 efi_status_t efi_free_pages(uint64_t memory, efi_uintn_t pages)
 {
 	uint64_t r = 0;
@@ -351,8 +367,15 @@ efi_status_t efi_free_pages(uint64_t memory, efi_uintn_t pages)
 	return EFI_NOT_FOUND;
 }
 
-efi_status_t efi_allocate_pool(int pool_type, efi_uintn_t size,
-			       void **buffer)
+/*
+ * Allocate memory from pool.
+ *
+ * @pool_type	type of the pool from which memory is to be allocated
+ * @size	number of bytes to be allocated
+ * @buffer	allocated memory
+ * @return	status code
+ */
+efi_status_t efi_allocate_pool(int pool_type, efi_uintn_t size, void **buffer)
 {
 	efi_status_t r;
 	efi_physical_addr_t t;
@@ -375,6 +398,12 @@ efi_status_t efi_allocate_pool(int pool_type, efi_uintn_t size,
 	return r;
 }
 
+/*
+ * Free memory from pool.
+ *
+ * @buffer	start of memory to be freed
+ * @return	status code
+ */
 efi_status_t efi_free_pool(void *buffer)
 {
 	efi_status_t r;
@@ -392,6 +421,17 @@ efi_status_t efi_free_pool(void *buffer)
 	return r;
 }
 
+/*
+ * Get map describing memory usage.
+ *
+ * @memory_map_size	on entry the size, in bytes, of the memory map buffer,
+ *			on exit the size of the copied memory map
+ * @memory_map		buffer to which the memory map is written
+ * @map_key		key for the memory map
+ * @descriptor_size	size of an individual memory descriptor
+ * @descriptor_version	version number of the memory descriptor structure
+ * @return		status code
+ */
 efi_status_t efi_get_memory_map(efi_uintn_t *memory_map_size,
 				struct efi_mem_desc *memory_map,
 				efi_uintn_t *map_key,
diff --git a/lib/efi_loader/helloworld.c b/lib/efi_loader/helloworld.c
index b8c147d7f2..1ec0179226 100644
--- a/lib/efi_loader/helloworld.c
+++ b/lib/efi_loader/helloworld.c
@@ -14,6 +14,22 @@
 #include <efi_api.h>
 
 static const efi_guid_t loaded_image_guid = LOADED_IMAGE_GUID;
+static const efi_guid_t fdt_guid = EFI_FDT_GUID;
+static const efi_guid_t smbios_guid = SMBIOS_TABLE_GUID;
+
+static int hw_memcmp(const void *buf1, const void *buf2, size_t length)
+{
+	const u8 *pos1 = buf1;
+	const u8 *pos2 = buf2;
+
+	for (; length; --length) {
+		if (*pos1 != *pos2)
+			return *pos1 - *pos2;
+		++pos1;
+		++pos2;
+	}
+	return 0;
+}
 
 /*
  * Entry point of the EFI application.
@@ -29,6 +45,7 @@ efi_status_t EFIAPI efi_main(efi_handle_t handle,
 	struct efi_boot_services *boottime = systable->boottime;
 	struct efi_loaded_image *loaded_image;
 	efi_status_t ret;
+	efi_uintn_t i;
 
 	con_out->output_string(con_out, L"Hello, world!\n");
 
@@ -40,6 +57,15 @@ efi_status_t EFIAPI efi_main(efi_handle_t handle,
 				       L"Cannot open loaded image protocol\n");
 		goto out;
 	}
+	/* Find configuration tables */
+	for (i = 0; i < systable->nr_tables; ++i) {
+		if (!hw_memcmp(&systable->tables[i].guid, &fdt_guid,
+			       sizeof(efi_guid_t)))
+			con_out->output_string(con_out, L"Have device tree\n");
+		if (!hw_memcmp(&systable->tables[i].guid, &smbios_guid,
+			       sizeof(efi_guid_t)))
+			con_out->output_string(con_out, L"Have SMBIOS table\n");
+	}
 	/* Output the load options */
 	con_out->output_string(con_out, L"Load options: ");
 	if (loaded_image->load_options_size && loaded_image->load_options)
diff --git a/lib/efi_selftest/.gitignore b/lib/efi_selftest/.gitignore
new file mode 100644
index 0000000000..c527e464e5
--- /dev/null
+++ b/lib/efi_selftest/.gitignore
@@ -0,0 +1,2 @@
+efi_miniapp_file_image.h
+*.efi
diff --git a/lib/efi_selftest/Makefile b/lib/efi_selftest/Makefile
index 837e86228e..90246f7827 100644
--- a/lib/efi_selftest/Makefile
+++ b/lib/efi_selftest/Makefile
@@ -7,8 +7,12 @@
 # This file only gets included with CONFIG_EFI_LOADER set, so all
 # object inclusion implicitly depends on it
 
+CFLAGS_efi_selftest_miniapp.o := $(CFLAGS_EFI) -Os -ffreestanding
+CFLAGS_REMOVE_efi_selftest_miniapp.o := $(CFLAGS_NON_EFI) -Os
+
 obj-$(CONFIG_CMD_BOOTEFI_SELFTEST) += \
 efi_selftest.o \
+efi_selftest_controllers.o \
 efi_selftest_console.o \
 efi_selftest_devicepath.o \
 efi_selftest_events.o \
@@ -20,3 +24,39 @@ efi_selftest_textoutput.o \
 efi_selftest_tpl.o \
 efi_selftest_util.o \
 efi_selftest_watchdog.o
+
+ifeq ($(CONFIG_BLK)$(CONFIG_PARTITIONS),yy)
+obj-$(CONFIG_CMD_BOOTEFI_SELFTEST) += efi_selftest_block_device.o
+endif
+
+# TODO: As of v2018.01 the relocation code for the EFI application cannot
+# be built on x86_64.
+ifeq ($(CONFIG_X86_64),)
+
+ifneq ($(CONFIG_CMD_BOOTEFI_SELFTEST),)
+
+obj-y += \
+efi_selftest_startimage_exit.o \
+efi_selftest_startimage_return.o
+
+targets += \
+efi_miniapp_file_image_exit.h \
+efi_miniapp_file_image_return.h \
+efi_selftest_miniapp_exit.efi \
+efi_selftest_miniapp_return.efi
+
+$(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_return.h: $(obj)/efi_selftest_miniapp_return.efi
+	$(obj)/../../tools/file2include $(obj)/efi_selftest_miniapp_return.efi > \
+	$(obj)/efi_miniapp_file_image_return.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
+
+endif
+
+endif
diff --git a/lib/efi_selftest/efi_selftest.c b/lib/efi_selftest/efi_selftest.c
index 4e5a12c47c..fc5ef254a1 100644
--- a/lib/efi_selftest/efi_selftest.c
+++ b/lib/efi_selftest/efi_selftest.c
@@ -65,7 +65,7 @@ void efi_st_exit_boot_services(void)
 		efi_st_error("ExitBootServices did not return EFI_SUCCESS\n");
 		return;
 	}
-	efi_st_printf("\nBoot services terminated\n");
+	efi_st_printc(EFI_WHITE, "\nBoot services terminated\n");
 }
 
 /*
@@ -81,13 +81,14 @@ static int setup(struct efi_unit_test *test, unsigned int *failures)
 
 	if (!test->setup)
 		return EFI_ST_SUCCESS;
-	efi_st_printf("\nSetting up '%s'\n", test->name);
+	efi_st_printc(EFI_LIGHTBLUE, "\nSetting up '%s'\n", test->name);
 	ret = test->setup(handle, systable);
 	if (ret != EFI_ST_SUCCESS) {
 		efi_st_error("Setting up '%s' failed\n", test->name);
 		++*failures;
 	} else {
-		efi_st_printf("Setting up '%s' succeeded\n", test->name);
+		efi_st_printc(EFI_LIGHTGREEN,
+			      "Setting up '%s' succeeded\n", test->name);
 	}
 	return ret;
 }
@@ -105,13 +106,14 @@ static int execute(struct efi_unit_test *test, unsigned int *failures)
 
 	if (!test->execute)
 		return EFI_ST_SUCCESS;
-	efi_st_printf("\nExecuting '%s'\n", test->name);
+	efi_st_printc(EFI_LIGHTBLUE, "\nExecuting '%s'\n", test->name);
 	ret = test->execute();
 	if (ret != EFI_ST_SUCCESS) {
 		efi_st_error("Executing '%s' failed\n", test->name);
 		++*failures;
 	} else {
-		efi_st_printf("Executing '%s' succeeded\n", test->name);
+		efi_st_printc(EFI_LIGHTGREEN,
+			      "Executing '%s' succeeded\n", test->name);
 	}
 	return ret;
 }
@@ -129,13 +131,14 @@ static int teardown(struct efi_unit_test *test, unsigned int *failures)
 
 	if (!test->teardown)
 		return EFI_ST_SUCCESS;
-	efi_st_printf("\nTearing down '%s'\n", test->name);
+	efi_st_printc(EFI_LIGHTBLUE, "\nTearing down '%s'\n", test->name);
 	ret = test->teardown();
 	if (ret != EFI_ST_SUCCESS) {
 		efi_st_error("Tearing down '%s' failed\n", test->name);
 		++*failures;
 	} else {
-		efi_st_printf("Tearing down '%s' succeeded\n", test->name);
+		efi_st_printc(EFI_LIGHTGREEN,
+			      "Tearing down '%s' succeeded\n", test->name);
 	}
 	return ret;
 }
@@ -262,12 +265,12 @@ efi_status_t EFIAPI efi_selftest(efi_handle_t image_handle,
 		}
 	}
 
-	efi_st_printf("\nTesting EFI API implementation\n");
+	efi_st_printc(EFI_WHITE, "\nTesting EFI API implementation\n");
 
 	if (testname)
-		efi_st_printf("\nSelected test: '%ps'\n", testname);
+		efi_st_printc(EFI_WHITE, "\nSelected test: '%ps'\n", testname);
 	else
-		efi_st_printf("\nNumber of tests to execute: %u\n",
+		efi_st_printc(EFI_WHITE, "\nNumber of tests to execute: %u\n",
 			      ll_entry_count(struct efi_unit_test,
 					     efi_unit_test));
 
@@ -291,7 +294,7 @@ efi_status_t EFIAPI efi_selftest(efi_handle_t image_handle,
 			&failures);
 
 	/* Give feedback */
-	efi_st_printf("\nSummary: %u failures\n\n", failures);
+	efi_st_printc(EFI_WHITE, "\nSummary: %u failures\n\n", failures);
 
 	/* Reset system */
 	efi_st_printf("Preparing for reset. Press any key.\n");
diff --git a/lib/efi_selftest/efi_selftest_block_device.c b/lib/efi_selftest/efi_selftest_block_device.c
new file mode 100644
index 0000000000..9e4b93d9a6
--- /dev/null
+++ b/lib/efi_selftest/efi_selftest_block_device.c
@@ -0,0 +1,395 @@
+/*
+ * efi_selftest_block
+ *
+ * Copyright (c) 2017 Heinrich Schuchardt <xypron.glpk@gmx.de>
+ *
+ * SPDX-License-Identifier:     GPL-2.0+
+ *
+ * This test checks the driver for block IO devices.
+ * A disk image is created in memory.
+ * A handle is created for the new block IO device.
+ * The block I/O protocol is installed on the handle.
+ * ConnectController is used to setup partitions and to install the simple
+ * file protocol.
+ * A known file is read from the file system and verified.
+ */
+
+#include <efi_selftest.h>
+#include "efi_selftest_disk_image.h"
+
+/* Block size of compressed disk image */
+#define COMPRESSED_DISK_IMAGE_BLOCK_SIZE 8
+
+/* Binary logarithm of the block size */
+#define LB_BLOCK_SIZE 9
+
+static struct efi_boot_services *boottime;
+
+static const efi_guid_t block_io_protocol_guid = BLOCK_IO_GUID;
+static const efi_guid_t guid_device_path = DEVICE_PATH_GUID;
+static const efi_guid_t guid_simple_file_system_protocol =
+					EFI_SIMPLE_FILE_SYSTEM_PROTOCOL_GUID;
+static efi_guid_t guid_vendor =
+	EFI_GUID(0xdbca4c98, 0x6cb0, 0x694d,
+		 0x08, 0x72, 0x81, 0x9c, 0x65, 0x0c, 0xb7, 0xb8);
+
+static struct efi_device_path *dp;
+
+/* One 8 byte block of the compressed disk image */
+struct line {
+	size_t addr;
+	char *line;
+};
+
+/* Compressed disk image */
+struct compressed_disk_image {
+	size_t length;
+	struct line lines[];
+};
+
+static const struct compressed_disk_image img = EFI_ST_DISK_IMG;
+
+/* Decompressed disk image */
+static u8 *image;
+
+/*
+ * Reset service of the block IO protocol.
+ *
+ * @this	block IO protocol
+ * @return	status code
+ */
+static efi_status_t EFIAPI reset(
+			struct efi_block_io *this,
+			char extended_verification)
+{
+	return EFI_SUCCESS;
+}
+
+/*
+ * Read service of the block IO protocol.
+ *
+ * @this	block IO protocol
+ * @media_id	media id
+ * @lba		start of the read in logical blocks
+ * @buffer_size	number of bytes to read
+ * @buffer	target buffer
+ * @return	status code
+ */
+static efi_status_t EFIAPI read_blocks(
+			struct efi_block_io *this, u32 media_id, u64 lba,
+			efi_uintn_t buffer_size, void *buffer)
+{
+	u8 *start;
+
+	if ((lba << LB_BLOCK_SIZE) + buffer_size > img.length)
+		return EFI_INVALID_PARAMETER;
+	start = image + (lba << LB_BLOCK_SIZE);
+
+	boottime->copy_mem(buffer, start, buffer_size);
+
+	return EFI_SUCCESS;
+}
+
+/*
+ * Write service of the block IO protocol.
+ *
+ * @this	block IO protocol
+ * @media_id	media id
+ * @lba		start of the write in logical blocks
+ * @buffer_size	number of bytes to read
+ * @buffer	source buffer
+ * @return	status code
+ */
+static efi_status_t EFIAPI write_blocks(
+			struct efi_block_io *this, u32 media_id, u64 lba,
+			efi_uintn_t buffer_size, void *buffer)
+{
+	u8 *start;
+
+	if ((lba << LB_BLOCK_SIZE) + buffer_size > img.length)
+		return EFI_INVALID_PARAMETER;
+	start = image + (lba << LB_BLOCK_SIZE);
+
+	boottime->copy_mem(start, buffer, buffer_size);
+
+	return EFI_SUCCESS;
+}
+
+/*
+ * Flush service of the block IO protocol.
+ *
+ * @this	block IO protocol
+ * @return	status code
+ */
+static efi_status_t EFIAPI flush_blocks(struct efi_block_io *this)
+{
+	return EFI_SUCCESS;
+}
+
+/*
+ * 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;
+}
+
+static struct efi_block_io_media media;
+
+static struct efi_block_io block_io = {
+	.media = &media,
+	.reset = reset,
+	.read_blocks = read_blocks,
+	.write_blocks = write_blocks,
+	.flush_blocks = flush_blocks,
+};
+
+/* Handle for the block IO device */
+static efi_handle_t disk_handle;
+
+/*
+ * Setup unit test.
+ *
+ * @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)
+{
+	efi_status_t ret;
+	struct efi_device_path_vendor vendor_node;
+	struct efi_device_path end_node;
+
+	boottime = systable->boottime;
+
+	decompress(&image);
+
+	block_io.media->block_size = 1 << LB_BLOCK_SIZE;
+	block_io.media->last_block = img.length >> LB_BLOCK_SIZE;
+
+	ret = boottime->install_protocol_interface(
+				&disk_handle, &block_io_protocol_guid,
+				EFI_NATIVE_INTERFACE, &block_io);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Failed to install block I/O protocol\n");
+		return EFI_ST_FAILURE;
+	}
+
+	ret = boottime->allocate_pool(EFI_LOADER_DATA,
+				      sizeof(struct efi_device_path_vendor) +
+				      sizeof(struct efi_device_path),
+				      (void **)&dp);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Out of memory\n");
+		return EFI_ST_FAILURE;
+	}
+	vendor_node.dp.type = DEVICE_PATH_TYPE_HARDWARE_DEVICE;
+	vendor_node.dp.sub_type = DEVICE_PATH_SUB_TYPE_VENDOR;
+	vendor_node.dp.length = sizeof(struct efi_device_path_vendor);
+
+	boottime->copy_mem(&vendor_node.guid, &guid_vendor,
+			   sizeof(efi_guid_t));
+	boottime->copy_mem(dp, &vendor_node,
+			   sizeof(struct efi_device_path_vendor));
+	end_node.type = DEVICE_PATH_TYPE_END;
+	end_node.sub_type = DEVICE_PATH_SUB_TYPE_END;
+	end_node.length = sizeof(struct efi_device_path);
+
+	boottime->copy_mem((char *)dp + sizeof(struct efi_device_path_vendor),
+			   &end_node, sizeof(struct efi_device_path));
+	ret = boottime->install_protocol_interface(&disk_handle,
+						   &guid_device_path,
+						   EFI_NATIVE_INTERFACE,
+						   dp);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("InstallProtocolInterface failed\n");
+		return EFI_ST_FAILURE;
+	}
+	return EFI_ST_SUCCESS;
+}
+
+/*
+ * Tear down unit test.
+ *
+ * @return:	EFI_ST_SUCCESS for success
+ */
+static int teardown(void)
+{
+	efi_status_t r = EFI_ST_SUCCESS;
+
+	if (disk_handle) {
+		r = boottime->uninstall_protocol_interface(disk_handle,
+							   &guid_device_path,
+							   dp);
+		if (r != EFI_SUCCESS) {
+			efi_st_error("Uninstall device path failed\n");
+			return EFI_ST_FAILURE;
+		}
+		r = boottime->uninstall_protocol_interface(
+				disk_handle, &block_io_protocol_guid,
+				&block_io);
+		if (r != EFI_SUCCESS) {
+			efi_st_todo(
+				"Failed to uninstall block I/O protocol\n");
+			return EFI_ST_SUCCESS;
+		}
+	}
+
+	if (image) {
+		r = efi_free_pool(image);
+		if (r != EFI_SUCCESS) {
+			efi_st_error("Failed to free image\n");
+			return EFI_ST_FAILURE;
+		}
+	}
+	return r;
+}
+
+/*
+ * Get length of device path without end tag.
+ *
+ * @dp		device path
+ * @return	length of device path in bytes
+ */
+static efi_uintn_t dp_size(struct efi_device_path *dp)
+{
+	struct efi_device_path *pos = dp;
+
+	while (pos->type != DEVICE_PATH_TYPE_END)
+		pos = (struct efi_device_path *)((char *)pos + pos->length);
+	return (char *)pos - (char *)dp;
+}
+
+/*
+ * Execute unit test.
+ *
+ * @return:	EFI_ST_SUCCESS for success
+ */
+static int execute(void)
+{
+	efi_status_t ret;
+	efi_uintn_t no_handles, i, len;
+	efi_handle_t *handles;
+	efi_handle_t handle_partition = NULL;
+	struct efi_device_path *dp_partition;
+	struct efi_simple_file_system_protocol *file_system;
+	struct efi_file_handle *root, *file;
+	u64 buf_size;
+	char buf[16] __aligned(ARCH_DMA_MINALIGN);
+
+	ret = boottime->connect_controller(disk_handle, NULL, NULL, 1);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Failed to connect controller\n");
+		return EFI_ST_FAILURE;
+	}
+	ret = boottime->locate_handle_buffer(
+				BY_PROTOCOL, &guid_device_path, NULL,
+				&no_handles, &handles);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Failed to locate handles\n");
+		return EFI_ST_FAILURE;
+	}
+	len = dp_size(dp);
+	for (i = 0; i < no_handles; ++i) {
+		ret = boottime->open_protocol(handles[i], &guid_device_path,
+					      (void **)&dp_partition,
+					      NULL, NULL,
+					      EFI_OPEN_PROTOCOL_GET_PROTOCOL);
+		if (ret != EFI_SUCCESS) {
+			efi_st_error("Failed to open device path protocol\n");
+			return EFI_ST_FAILURE;
+		}
+		if (len >= dp_size(dp_partition))
+			continue;
+		if (efi_st_memcmp(dp, dp_partition, len))
+			continue;
+		handle_partition = handles[i];
+		break;
+	}
+	ret = boottime->free_pool(handles);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Failed to free pool memory\n");
+		return EFI_ST_FAILURE;
+	}
+	if (!handle_partition) {
+		efi_st_error("Partition handle not found\n");
+		return EFI_ST_FAILURE;
+	}
+	ret = boottime->open_protocol(handle_partition,
+				      &guid_simple_file_system_protocol,
+				      (void **)&file_system, NULL, NULL,
+				      EFI_OPEN_PROTOCOL_GET_PROTOCOL);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Failed to open simple file system protocol\n");
+		return EFI_ST_FAILURE;
+	}
+	ret = file_system->open_volume(file_system, &root);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Failed to open volume\n");
+		return EFI_ST_FAILURE;
+	}
+	ret = root->open(root, &file, (s16 *)L"hello.txt", EFI_FILE_MODE_READ,
+			 0);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Failed to open file\n");
+		return EFI_ST_FAILURE;
+	}
+	buf_size = sizeof(buf) - 1;
+	ret = file->read(file, &buf_size, buf);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Failed to read file\n");
+		return EFI_ST_FAILURE;
+	}
+	if (efi_st_memcmp(buf, "Hello world!", 12)) {
+		efi_st_error("Unexpected file content\n");
+		return EFI_ST_FAILURE;
+	}
+	ret = file->close(file);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Failed to close file\n");
+		return EFI_ST_FAILURE;
+	}
+	ret = root->close(root);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Failed to close volume\n");
+		return EFI_ST_FAILURE;
+	}
+
+	return EFI_ST_SUCCESS;
+}
+
+EFI_UNIT_TEST(blkdev) = {
+	.name = "block device",
+	.phase = EFI_EXECUTE_BEFORE_BOOTTIME_EXIT,
+	.setup = setup,
+	.execute = execute,
+	.teardown = teardown,
+};
diff --git a/lib/efi_selftest/efi_selftest_console.c b/lib/efi_selftest/efi_selftest_console.c
index 6a7fd20da5..e1649f48bc 100644
--- a/lib/efi_selftest/efi_selftest_console.c
+++ b/lib/efi_selftest/efi_selftest_console.c
@@ -130,22 +130,25 @@ static void int2dec(s32 value, u16 **buf)
 }
 
 /*
- * Print a formatted string to the EFI console
+ * Print a colored formatted string to the EFI console
  *
- * @fmt: format string
- * @...: optional arguments
+ * @color	color, see constants in efi_api.h, use -1 for no color
+ * @fmt		format string
+ * @...		optional arguments
  */
-void efi_st_printf(const char *fmt, ...)
+void efi_st_printc(int color, const char *fmt, ...)
 {
 	va_list args;
 	u16 buf[160];
 	const char *c;
 	u16 *pos = buf;
 	const char *s;
-	const u16 *u;
+	u16 *u;
 
 	va_start(args, fmt);
 
+	if (color >= 0)
+		con_out->set_attribute(con_out, (unsigned long)color);
 	c = fmt;
 	for (; *c; ++c) {
 		switch (*c) {
@@ -188,9 +191,13 @@ void efi_st_printf(const char *fmt, ...)
 				/* u16 string */
 				case 's':
 					u = va_arg(args, u16*);
-					/* Ensure string fits into buffer */
-					for (; *u && pos < buf + 120; ++u)
-						*pos++ = *u;
+					if (pos > buf) {
+						*pos = 0;
+						con_out->output_string(con_out,
+								       buf);
+					}
+					con_out->output_string(con_out, u);
+					pos = buf;
 					break;
 				default:
 					--c;
@@ -216,6 +223,8 @@ void efi_st_printf(const char *fmt, ...)
 	va_end(args);
 	*pos = 0;
 	con_out->output_string(con_out, buf);
+	if (color >= 0)
+		con_out->set_attribute(con_out, EFI_LIGHTGRAY);
 }
 
 /*
diff --git a/lib/efi_selftest/efi_selftest_controllers.c b/lib/efi_selftest/efi_selftest_controllers.c
new file mode 100644
index 0000000000..1a22aba6f4
--- /dev/null
+++ b/lib/efi_selftest/efi_selftest_controllers.c
@@ -0,0 +1,385 @@
+/*
+ * efi_selftest_controllers
+ *
+ * Copyright (c) 2017 Heinrich Schuchardt <xypron.glpk@gmx.de>
+ *
+ * SPDX-License-Identifier:     GPL-2.0+
+ *
+ * This unit test checks the following protocol services:
+ * ConnectController, DisconnectController,
+ * InstallProtocol, UninstallProtocol,
+ * OpenProtocol, CloseProtcol, OpenProtocolInformation
+ */
+
+#include <efi_selftest.h>
+
+#define NUMBER_OF_CHILD_CONTROLLERS 4
+
+static struct efi_boot_services *boottime;
+const efi_guid_t guid_driver_binding_protocol =
+			EFI_DRIVER_BINDING_PROTOCOL_GUID;
+static efi_guid_t guid_controller =
+	EFI_GUID(0xe6ab1d96, 0x6bff, 0xdb42,
+		 0xaa, 0x05, 0xc8, 0x1f, 0x7f, 0x45, 0x26, 0x34);
+static efi_guid_t guid_child_controller =
+	EFI_GUID(0x1d41f6f5, 0x2c41, 0xddfb,
+		 0xe2, 0x9b, 0xb8, 0x0e, 0x2e, 0xe8, 0x3a, 0x85);
+static efi_handle_t handle_controller;
+static efi_handle_t handle_child_controller[NUMBER_OF_CHILD_CONTROLLERS];
+static efi_handle_t handle_driver;
+
+/*
+ * Count child controllers
+ *
+ * @handle	handle on which child controllers are installed
+ * @protocol	protocol for which the child controlles where installed
+ * @count	number of child controllers
+ * @return	status code
+ */
+static efi_status_t count_child_controllers(efi_handle_t handle,
+					    efi_guid_t *protocol,
+					    efi_uintn_t *count)
+{
+	efi_status_t ret;
+	efi_uintn_t entry_count;
+	struct efi_open_protocol_info_entry *entry_buffer;
+
+	*count = 0;
+	ret = boottime->open_protocol_information(handle, protocol,
+						  &entry_buffer, &entry_count);
+	if (ret != EFI_SUCCESS)
+		return ret;
+	if (!entry_count)
+		return EFI_SUCCESS;
+	while (entry_count) {
+		if (entry_buffer[--entry_count].attributes &
+		    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER)
+			++*count;
+	}
+	ret = boottime->free_pool(entry_buffer);
+	if (ret != EFI_SUCCESS)
+		efi_st_error("Cannot free buffer\n");
+	return ret;
+}
+
+/*
+ * Check if the driver supports the controller.
+ *
+ * @this			driver binding protocol
+ * @controller_handle		handle of the controller
+ * @remaining_device_path	path specifying the child controller
+ * @return			status code
+ */
+static efi_status_t EFIAPI supported(
+		struct efi_driver_binding_protocol *this,
+		efi_handle_t controller_handle,
+		struct efi_device_path *remaining_device_path)
+{
+	efi_status_t ret;
+	void *interface;
+
+	ret = boottime->open_protocol(
+			controller_handle, &guid_controller,
+			&interface, handle_driver,
+			controller_handle, EFI_OPEN_PROTOCOL_BY_DRIVER);
+	switch (ret) {
+	case EFI_ACCESS_DENIED:
+	case EFI_ALREADY_STARTED:
+		return ret;
+	case EFI_SUCCESS:
+		break;
+	default:
+		return EFI_UNSUPPORTED;
+	}
+	ret = boottime->close_protocol(
+				controller_handle, &guid_controller,
+				handle_driver, controller_handle);
+	if (ret != EFI_SUCCESS)
+		ret = EFI_UNSUPPORTED;
+	return ret;
+}
+
+/*
+ * Create child controllers and attach driver.
+ *
+ * @this			driver binding protocol
+ * @controller_handle		handle of the controller
+ * @remaining_device_path	path specifying the child controller
+ * @return			status code
+ */
+static efi_status_t EFIAPI start(
+		struct efi_driver_binding_protocol *this,
+		efi_handle_t controller_handle,
+		struct efi_device_path *remaining_device_path)
+{
+	size_t i;
+	efi_status_t ret;
+	void *interface;
+
+	/* Attach driver to controller */
+	ret = boottime->open_protocol(
+			controller_handle, &guid_controller,
+			&interface, handle_driver,
+			controller_handle, EFI_OPEN_PROTOCOL_BY_DRIVER);
+	switch (ret) {
+	case EFI_ACCESS_DENIED:
+	case EFI_ALREADY_STARTED:
+		return ret;
+	case EFI_SUCCESS:
+		break;
+	default:
+		return EFI_UNSUPPORTED;
+	}
+
+	/* Create child controllers */
+	for (i = 0; i < NUMBER_OF_CHILD_CONTROLLERS; ++i) {
+		ret = boottime->install_protocol_interface(
+			&handle_child_controller[i], &guid_child_controller,
+			EFI_NATIVE_INTERFACE, NULL);
+		if (ret != EFI_SUCCESS) {
+			efi_st_error("InstallProtocolInterface failed\n");
+			return EFI_ST_FAILURE;
+		}
+		ret = boottime->open_protocol(
+			controller_handle, &guid_controller,
+			&interface, handle_child_controller[i],
+			handle_child_controller[i],
+			EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER);
+		if (ret != EFI_SUCCESS) {
+			efi_st_error("OpenProtocol failed\n");
+			return EFI_ST_FAILURE;
+		}
+	}
+	return ret;
+}
+
+/*
+ * Remove a single child controller from the parent controller.
+ *
+ * @controller_handle	parent controller
+ * @child_handle	child controller
+ * @return		status code
+ */
+static efi_status_t disconnect_child(efi_handle_t controller_handle,
+				     efi_handle_t child_handle)
+{
+	efi_status_t ret;
+
+	ret = boottime->close_protocol(
+				controller_handle, &guid_controller,
+				child_handle, child_handle);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Cannot close protocol\n");
+		return ret;
+	}
+	ret = boottime->uninstall_protocol_interface(
+				child_handle, &guid_child_controller, NULL);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Cannot uninstall protocol interface\n");
+		return ret;
+	}
+	return ret;
+}
+
+/*
+ * Remove child controllers and disconnect the controller.
+ *
+ * @this			driver binding protocol
+ * @controller_handle		handle of the controller
+ * @number_of_children		number of child controllers to remove
+ * @child_handle_buffer		handles of the child controllers to remove
+ * @return			status code
+ */
+static efi_status_t EFIAPI stop(
+		struct efi_driver_binding_protocol *this,
+		efi_handle_t controller_handle,
+		size_t number_of_children,
+		efi_handle_t *child_handle_buffer)
+{
+	efi_status_t ret;
+	efi_uintn_t count;
+	struct efi_open_protocol_info_entry *entry_buffer;
+
+	/* Destroy provided child controllers */
+	if (number_of_children) {
+		efi_uintn_t i;
+
+		for (i = 0; i < number_of_children; ++i) {
+			ret = disconnect_child(controller_handle,
+					       child_handle_buffer[i]);
+			if (ret != EFI_SUCCESS)
+				return ret;
+		}
+		return EFI_SUCCESS;
+	}
+
+	/* Destroy all children */
+	ret = boottime->open_protocol_information(
+					controller_handle, &guid_controller,
+					&entry_buffer, &count);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("OpenProtocolInformation failed\n");
+		return ret;
+	}
+	while (count) {
+		if (entry_buffer[--count].attributes &
+		    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER) {
+			ret = disconnect_child(
+					controller_handle,
+					entry_buffer[count].agent_handle);
+			if (ret != EFI_SUCCESS)
+				return ret;
+		}
+	}
+	ret = boottime->free_pool(entry_buffer);
+	if (ret != EFI_SUCCESS)
+		efi_st_error("Cannot free buffer\n");
+
+	/* Detach driver from controller */
+	ret = boottime->close_protocol(
+			controller_handle, &guid_controller,
+			handle_driver, controller_handle);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Cannot close protocol\n");
+		return ret;
+	}
+	return EFI_SUCCESS;
+}
+
+/* Driver binding protocol interface */
+static struct efi_driver_binding_protocol binding_interface = {
+	supported,
+	start,
+	stop,
+	0xffffffff,
+	NULL,
+	NULL,
+	};
+
+/*
+ * Setup unit test.
+ *
+ * @handle	handle of the loaded image
+ * @systable	system table
+ */
+static int setup(const efi_handle_t img_handle,
+		 const struct efi_system_table *systable)
+{
+	efi_status_t ret;
+
+	boottime = systable->boottime;
+
+	/* Create controller handle */
+	ret = boottime->install_protocol_interface(
+			&handle_controller, &guid_controller,
+			EFI_NATIVE_INTERFACE, NULL);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("InstallProtocolInterface failed\n");
+		return EFI_ST_FAILURE;
+	}
+	/* Create driver handle */
+	ret = boottime->install_protocol_interface(
+			&handle_driver,  &guid_driver_binding_protocol,
+			EFI_NATIVE_INTERFACE, &binding_interface);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("InstallProtocolInterface failed\n");
+		return EFI_ST_FAILURE;
+	}
+
+	return EFI_ST_SUCCESS;
+}
+
+/*
+ * Execute unit test.
+ *
+ * The number of child controllers is checked after each of the following
+ * actions:
+ *
+ * Connect a controller to a driver.
+ * Disconnect and destroy a child controller.
+ * Disconnect and destroy the remaining child controllers.
+ *
+ * Connect a controller to a driver.
+ * Uninstall the driver protocol from the controller.
+ */
+static int execute(void)
+{
+	efi_status_t ret;
+	efi_uintn_t count;
+
+	/* Connect controller to driver */
+	ret = boottime->connect_controller(handle_controller, NULL, NULL, 1);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Failed to connect controller\n");
+		return EFI_ST_FAILURE;
+	}
+	/* Check number of child controllers */
+	ret = count_child_controllers(handle_controller, &guid_controller,
+				      &count);
+	if (ret != EFI_SUCCESS || count != NUMBER_OF_CHILD_CONTROLLERS) {
+		efi_st_error("Number of children %u != %u\n",
+			     (unsigned int)count, NUMBER_OF_CHILD_CONTROLLERS);
+	}
+	/* Destroy second child controller */
+	ret = boottime->disconnect_controller(handle_controller,
+					      handle_driver,
+					      handle_child_controller[1]);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Failed to disconnect child controller\n");
+		return EFI_ST_FAILURE;
+	}
+	/* Check number of child controllers */
+	ret = count_child_controllers(handle_controller, &guid_controller,
+				      &count);
+	if (ret != EFI_SUCCESS || count != NUMBER_OF_CHILD_CONTROLLERS - 1) {
+		efi_st_error("Destroying single child controller failed\n");
+		return EFI_ST_FAILURE;
+	}
+	/* Destroy remaining child controllers and disconnect controller */
+	ret = boottime->disconnect_controller(handle_controller, NULL, NULL);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Failed to disconnect controller\n");
+		return EFI_ST_FAILURE;
+	}
+	/* Check number of child controllers */
+	ret = count_child_controllers(handle_controller, &guid_controller,
+				      &count);
+	if (ret != EFI_SUCCESS || count) {
+		efi_st_error("Destroying child controllers failed\n");
+		return EFI_ST_FAILURE;
+	}
+
+	/* Connect controller to driver */
+	ret = boottime->connect_controller(handle_controller, NULL, NULL, 1);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Failed to connect controller\n");
+		return EFI_ST_FAILURE;
+	}
+	/* Check number of child controllers */
+	ret = count_child_controllers(handle_controller, &guid_controller,
+				      &count);
+	if (ret != EFI_SUCCESS || count != NUMBER_OF_CHILD_CONTROLLERS) {
+		efi_st_error("Number of children %u != %u\n",
+			     (unsigned int)count, NUMBER_OF_CHILD_CONTROLLERS);
+	}
+	/* Uninstall controller protocol */
+	ret = boottime->uninstall_protocol_interface(handle_controller,
+						     &guid_controller, NULL);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Failed to uninstall protocols\n");
+		return EFI_ST_FAILURE;
+	}
+	/* Check number of child controllers */
+	ret = count_child_controllers(handle_controller, &guid_controller,
+				      &count);
+	if (ret == EFI_SUCCESS)
+		efi_st_error("Uninstall failed\n");
+	return EFI_ST_SUCCESS;
+}
+
+EFI_UNIT_TEST(controllers) = {
+	.name = "controllers",
+	.phase = EFI_EXECUTE_BEFORE_BOOTTIME_EXIT,
+	.setup = setup,
+	.execute = execute,
+};
diff --git a/lib/efi_selftest/efi_selftest_devicepath.c b/lib/efi_selftest/efi_selftest_devicepath.c
index 1ab54ebb37..92940c7ab6 100644
--- a/lib/efi_selftest/efi_selftest_devicepath.c
+++ b/lib/efi_selftest/efi_selftest_devicepath.c
@@ -192,31 +192,41 @@ static int teardown(void)
 {
 	efi_status_t ret;
 
-	ret = boottime->uninstall_protocol_interface(&handle1,
+	ret = boottime->uninstall_protocol_interface(handle1,
 						     &guid_device_path,
 						     dp1);
-	if (ret != EFI_SUCCESS)
-		efi_st_todo("UninstallProtocolInterface failed\n");
-	ret = boottime->uninstall_protocol_interface(&handle1,
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("UninstallProtocolInterface failed\n");
+		return EFI_ST_FAILURE;
+	}
+	ret = boottime->uninstall_protocol_interface(handle1,
 						     &guid_protocol,
 						     &interface);
-	if (ret != EFI_SUCCESS)
-		efi_st_todo("UninstallProtocolInterface failed\n");
-	ret = boottime->uninstall_protocol_interface(&handle2,
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("UninstallProtocolInterface failed\n");
+		return EFI_ST_FAILURE;
+	}
+	ret = boottime->uninstall_protocol_interface(handle2,
 						     &guid_device_path,
 						     dp2);
-	if (ret != EFI_SUCCESS)
-		efi_st_todo("UninstallProtocolInterface failed\n");
-	ret = boottime->uninstall_protocol_interface(&handle2,
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("UninstallProtocolInterface failed\n");
+		return EFI_ST_FAILURE;
+	}
+	ret = boottime->uninstall_protocol_interface(handle2,
 						     &guid_protocol,
 						     &interface);
-	if (ret != EFI_SUCCESS)
-		efi_st_todo("UninstallProtocolInterface failed\n");
-	ret = boottime->uninstall_protocol_interface(&handle3,
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("UninstallProtocolInterface failed\n");
+		return EFI_ST_FAILURE;
+	}
+	ret = boottime->uninstall_protocol_interface(handle3,
 						     &guid_device_path,
 						     dp3);
-	if (ret != EFI_SUCCESS)
-		efi_st_todo("UninstallProtocolInterface failed\n");
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("UninstallProtocolInterface failed\n");
+		return EFI_ST_FAILURE;
+	}
 	if (dp1) {
 		ret = boottime->free_pool(dp1);
 		if (ret != EFI_SUCCESS) {
@@ -299,17 +309,16 @@ static int execute(void)
 			efi_st_error("FreePool failed\n");
 			return EFI_ST_FAILURE;
 		}
-		ret = boottime->close_protocol(handles[i], &guid_device_path,
-					       NULL, NULL);
-		if (ret != EFI_SUCCESS)
-			efi_st_todo("Cannot close device path protocol.\n");
+		/*
+		 * CloseProtocol cannot be called without agent handle.
+		 * There is no need to close the device path protocol.
+		 */
 	}
 	ret = boottime->free_pool(handles);
 	if (ret != EFI_SUCCESS) {
 		efi_st_error("FreePool failed\n");
 		return EFI_ST_FAILURE;
 	}
-	efi_st_printf("\n");
 
 	/* Test ConvertDevicePathToText */
 	string = device_path_to_text->convert_device_path_to_text(
@@ -318,15 +327,14 @@ static int execute(void)
 		efi_st_error("ConvertDevicePathToText failed\n");
 		return EFI_ST_FAILURE;
 	}
-	efi_st_printf("dp2: %ps\n", string);
 	if (efi_st_strcmp_16_8(
 		string,
 		"/VenHw(dbca4c98-6cb0-694d-0872-819c650cbbb1)/VenHw(dbca4c98-6cb0-694d-0872-819c650cbba2)")
 	    ) {
+		efi_st_printf("dp2: %ps\n", string);
 		efi_st_error("Incorrect text from ConvertDevicePathToText\n");
 		return EFI_ST_FAILURE;
 	}
-
 	ret = boottime->free_pool(string);
 	if (ret != EFI_SUCCESS) {
 		efi_st_error("FreePool failed\n");
@@ -340,17 +348,17 @@ static int execute(void)
 		efi_st_error("ConvertDeviceNodeToText failed\n");
 		return EFI_ST_FAILURE;
 	}
-	efi_st_printf("dp_node: %ps\n", string);
-	ret = boottime->free_pool(string);
-	if (ret != EFI_SUCCESS) {
-		efi_st_error("FreePool failed\n");
-		return EFI_ST_FAILURE;
-	}
 	if (efi_st_strcmp_16_8(string, "u-boot")) {
+		efi_st_printf("dp_node: %ps\n", string);
 		efi_st_error(
 			"Incorrect conversion by ConvertDeviceNodeToText\n");
 		return EFI_ST_FAILURE;
 	}
+	ret = boottime->free_pool(string);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("FreePool failed\n");
+		return EFI_ST_FAILURE;
+	}
 
 	/* Test LocateDevicePath */
 	remaining_dp = (struct efi_device_path *)dp3;
@@ -370,13 +378,18 @@ static int execute(void)
 		efi_st_error("ConvertDevicePathToText failed\n");
 		return EFI_ST_FAILURE;
 	}
-	efi_st_printf("remaining device path: %ps\n", string);
 	if (efi_st_strcmp_16_8(string,
 			       "/VenHw(dbca4c98-6cb0-694d-0872-819c650cbbc3)")
 	    ) {
+		efi_st_printf("remaining device path: %ps\n", string);
 		efi_st_error("LocateDevicePath: wrong remaining device path\n");
 		return EFI_ST_FAILURE;
 	}
+	ret = boottime->free_pool(string);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("FreePool failed\n");
+		return EFI_ST_FAILURE;
+	}
 
 	return EFI_ST_SUCCESS;
 }
diff --git a/lib/efi_selftest/efi_selftest_disk_image.h b/lib/efi_selftest/efi_selftest_disk_image.h
new file mode 100644
index 0000000000..4775dace70
--- /dev/null
+++ b/lib/efi_selftest/efi_selftest_disk_image.h
@@ -0,0 +1,69 @@
+/*
+ *  Non-zero 8 byte strings of a disk image
+ *
+ *  Generated with tools/file2include
+ *
+ *  SPDX-License-Identifier:     GPL-2.0+
+ */
+
+#define EFI_ST_DISK_IMG { 0x00010000, { \
+	{0x000001b8, "\x94\x37\x69\xfc\x00\x00\x00\x00"}, /* .7i..... */ \
+	{0x000001c0, "\x02\x00\x83\x02\x02\x00\x01\x00"}, /* ........ */ \
+	{0x000001c8, "\x00\x00\x7f\x00\x00\x00\x00\x00"}, /* ........ */ \
+	{0x000001f8, "\x00\x00\x00\x00\x00\x00\x55\xaa"}, /* ......U. */ \
+	{0x00000200, "\xeb\x3c\x90\x6d\x6b\x66\x73\x2e"}, /* .<.mkfs. */ \
+	{0x00000208, "\x66\x61\x74\x00\x02\x04\x01\x00"}, /* fat..... */ \
+	{0x00000210, "\x02\x00\x02\x7f\x00\xf8\x01\x00"}, /* ........ */ \
+	{0x00000218, "\x20\x00\x40\x00\x00\x00\x00\x00"}, /*  .@..... */ \
+	{0x00000220, "\x00\x00\x00\x00\x80\x00\x29\x86"}, /* ......). */ \
+	{0x00000228, "\xe8\x82\x80\x4e\x4f\x20\x4e\x41"}, /* ...NO NA */ \
+	{0x00000230, "\x4d\x45\x20\x20\x20\x20\x46\x41"}, /* ME    FA */ \
+	{0x00000238, "\x54\x31\x32\x20\x20\x20\x0e\x1f"}, /* T12   .. */ \
+	{0x00000240, "\xbe\x5b\x7c\xac\x22\xc0\x74\x0b"}, /* .[|.".t. */ \
+	{0x00000248, "\x56\xb4\x0e\xbb\x07\x00\xcd\x10"}, /* V....... */ \
+	{0x00000250, "\x5e\xeb\xf0\x32\xe4\xcd\x16\xcd"}, /* ^..2.... */ \
+	{0x00000258, "\x19\xeb\xfe\x54\x68\x69\x73\x20"}, /* ...This  */ \
+	{0x00000260, "\x69\x73\x20\x6e\x6f\x74\x20\x61"}, /* is not a */ \
+	{0x00000268, "\x20\x62\x6f\x6f\x74\x61\x62\x6c"}, /*  bootabl */ \
+	{0x00000270, "\x65\x20\x64\x69\x73\x6b\x2e\x20"}, /* e disk.  */ \
+	{0x00000278, "\x20\x50\x6c\x65\x61\x73\x65\x20"}, /*  Please  */ \
+	{0x00000280, "\x69\x6e\x73\x65\x72\x74\x20\x61"}, /* insert a */ \
+	{0x00000288, "\x20\x62\x6f\x6f\x74\x61\x62\x6c"}, /*  bootabl */ \
+	{0x00000290, "\x65\x20\x66\x6c\x6f\x70\x70\x79"}, /* e floppy */ \
+	{0x00000298, "\x20\x61\x6e\x64\x0d\x0a\x70\x72"}, /*  and..pr */ \
+	{0x000002a0, "\x65\x73\x73\x20\x61\x6e\x79\x20"}, /* ess any  */ \
+	{0x000002a8, "\x6b\x65\x79\x20\x74\x6f\x20\x74"}, /* key to t */ \
+	{0x000002b0, "\x72\x79\x20\x61\x67\x61\x69\x6e"}, /* ry again */ \
+	{0x000002b8, "\x20\x2e\x2e\x2e\x20\x0d\x0a\x00"}, /*  ... ... */ \
+	{0x000003f8, "\x00\x00\x00\x00\x00\x00\x55\xaa"}, /* ......U. */ \
+	{0x00000400, "\xf8\xff\xff\x00\x00\x00\x00\xf0"}, /* ........ */ \
+	{0x00000408, "\xff\x00\x00\x00\x00\x00\x00\x00"}, /* ........ */ \
+	{0x00000600, "\xf8\xff\xff\x00\x00\x00\x00\xf0"}, /* ........ */ \
+	{0x00000608, "\xff\x00\x00\x00\x00\x00\x00\x00"}, /* ........ */ \
+	{0x00000800, "\xe5\x70\x00\x00\x00\xff\xff\xff"}, /* .p...... */ \
+	{0x00000808, "\xff\xff\xff\x0f\x00\x0e\xff\xff"}, /* ........ */ \
+	{0x00000810, "\xff\xff\xff\xff\xff\xff\xff\xff"}, /* ........ */ \
+	{0x00000818, "\xff\xff\x00\x00\xff\xff\xff\xff"}, /* ........ */ \
+	{0x00000820, "\xe5\x2e\x00\x68\x00\x65\x00\x6c"}, /* ...h.e.l */ \
+	{0x00000828, "\x00\x6c\x00\x0f\x00\x0e\x6f\x00"}, /* .l....o. */ \
+	{0x00000830, "\x2e\x00\x74\x00\x78\x00\x74\x00"}, /* ..t.x.t. */ \
+	{0x00000838, "\x2e\x00\x00\x00\x73\x00\x77\x00"}, /* ....s.w. */ \
+	{0x00000840, "\xe5\x45\x4c\x4c\x4f\x54\x7e\x31"}, /* .ELLOT~1 */ \
+	{0x00000848, "\x53\x57\x50\x20\x00\x64\xd0\x8a"}, /* SWP .d.. */ \
+	{0x00000850, "\x92\x4b\x92\x4b\x00\x00\xd0\x8a"}, /* .K.K.... */ \
+	{0x00000858, "\x92\x4b\x00\x00\x00\x00\x00\x00"}, /* .K...... */ \
+	{0x00000860, "\x41\x68\x00\x65\x00\x6c\x00\x6c"}, /* Ah.e.l.l */ \
+	{0x00000868, "\x00\x6f\x00\x0f\x00\xf1\x2e\x00"}, /* .o...... */ \
+	{0x00000870, "\x74\x00\x78\x00\x74\x00\x00\x00"}, /* t.x.t... */ \
+	{0x00000878, "\xff\xff\x00\x00\xff\xff\xff\xff"}, /* ........ */ \
+	{0x00000880, "\x48\x45\x4c\x4c\x4f\x20\x20\x20"}, /* HELLO    */ \
+	{0x00000888, "\x54\x58\x54\x20\x00\x64\xd4\x8a"}, /* TXT .d.. */ \
+	{0x00000890, "\x92\x4b\x92\x4b\x00\x00\xd4\x8a"}, /* .K.K.... */ \
+	{0x00000898, "\x92\x4b\x05\x00\x0d\x00\x00\x00"}, /* .K...... */ \
+	{0x000008a0, "\xe5\x45\x4c\x4c\x4f\x54\x7e\x31"}, /* .ELLOT~1 */ \
+	{0x000008a8, "\x53\x57\x58\x20\x00\x64\xd0\x8a"}, /* SWX .d.. */ \
+	{0x000008b0, "\x92\x4b\x92\x4b\x00\x00\xd0\x8a"}, /* .K.K.... */ \
+	{0x000008b8, "\x92\x4b\x00\x00\x00\x00\x00\x00"}, /* .K...... */ \
+	{0x00006000, "\x48\x65\x6c\x6c\x6f\x20\x77\x6f"}, /* Hello wo */ \
+	{0x00006008, "\x72\x6c\x64\x21\x0a\x00\x00\x00"}, /* rld!.... */ \
+	{0, NULL} } }
diff --git a/lib/efi_selftest/efi_selftest_events.c b/lib/efi_selftest/efi_selftest_events.c
index ad9490bd25..5393e39352 100644
--- a/lib/efi_selftest/efi_selftest_events.c
+++ b/lib/efi_selftest/efi_selftest_events.c
@@ -142,8 +142,8 @@ static int execute(void)
 		efi_st_error("WaitForEvent returned wrong index\n");
 		return EFI_ST_FAILURE;
 	}
-	efi_st_printf("Notification count periodic: %u\n", timer_ticks);
 	if (timer_ticks < 8 || timer_ticks > 12) {
+		efi_st_printf("Notification count periodic: %u\n", timer_ticks);
 		efi_st_error("Incorrect timing of events\n");
 		return EFI_ST_FAILURE;
 	}
@@ -170,8 +170,9 @@ static int execute(void)
 		efi_st_error("Could not wait for event\n");
 		return EFI_ST_FAILURE;
 	}
-	efi_st_printf("Notification count single shot: %u\n", timer_ticks);
 	if (timer_ticks != 1) {
+		efi_st_printf("Notification count single shot: %u\n",
+			      timer_ticks);
 		efi_st_error("Single shot timer failed\n");
 		return EFI_ST_FAILURE;
 	}
@@ -180,8 +181,9 @@ static int execute(void)
 		efi_st_error("Could not wait for event\n");
 		return EFI_ST_FAILURE;
 	}
-	efi_st_printf("Notification count stopped timer: %u\n", timer_ticks);
 	if (timer_ticks != 1) {
+		efi_st_printf("Notification count stopped timer: %u\n",
+			      timer_ticks);
 		efi_st_error("Stopped timer fired\n");
 		return EFI_ST_FAILURE;
 	}
diff --git a/lib/efi_selftest/efi_selftest_manageprotocols.c b/lib/efi_selftest/efi_selftest_manageprotocols.c
index f20f1528d4..874f86102d 100644
--- a/lib/efi_selftest/efi_selftest_manageprotocols.c
+++ b/lib/efi_selftest/efi_selftest_manageprotocols.c
@@ -194,7 +194,7 @@ static int execute(void)
 						&guid3, &interface3,
 						NULL);
 	if (ret == EFI_SUCCESS) {
-		efi_st_todo("UninstallMultipleProtocolInterfaces did not catch error\n");
+		efi_st_error("UninstallMultipleProtocolInterfaces did not catch error\n");
 		return EFI_ST_FAILURE;
 	}
 
@@ -273,8 +273,8 @@ static int execute(void)
 						&guid2, &interface2,
 						NULL);
 	if (ret != EFI_SUCCESS) {
-		efi_st_todo("UninstallMultipleProtocolInterfaces failed\n");
-		/* This test is known to fail due to missing implementation */
+		efi_st_error("UninstallMultipleProtocolInterfaces failed\n");
+		return EFI_ST_FAILURE;
 	}
 	/*
 	 * Check that the protocols are really uninstalled.
@@ -287,8 +287,8 @@ static int execute(void)
 		return EFI_ST_FAILURE;
 	}
 	if (count != 1) {
-		efi_st_todo("UninstallMultipleProtocolInterfaces failed to uninstall protocols\n");
-		/* This test is known to fail due to missing implementation */
+		efi_st_error("UninstallMultipleProtocolInterfaces failed to uninstall protocols\n");
+		return EFI_ST_FAILURE;
 	}
 	ret = find_in_buffer(handle1, count, buffer);
 	if (ret != EFI_SUCCESS) {
@@ -327,19 +327,19 @@ static int execute(void)
 	ret = boottime->uninstall_protocol_interface(handle1, &guid1,
 						     &interface1);
 	if (ret != EFI_SUCCESS) {
-		efi_st_todo("UninstallProtocolInterface failed\n");
-		/* This test is known to fail due to missing implementation */
+		efi_st_error("UninstallProtocolInterface failed\n");
+		return EFI_ST_FAILURE;
 	}
 	ret = boottime->handle_protocol(handle1, &guid1, (void **)&interface);
 	if (ret == EFI_SUCCESS) {
-		efi_st_todo("UninstallProtocolInterface failed\n");
-		/* This test is known to fail due to missing implementation */
+		efi_st_error("UninstallProtocolInterface failed\n");
+		return EFI_ST_FAILURE;
 	}
 	ret = boottime->uninstall_protocol_interface(handle1, &guid3,
 						     &interface1);
 	if (ret != EFI_SUCCESS) {
-		efi_st_todo("UninstallProtocolInterface failed\n");
-		/* This test is known to fail due to missing implementation */
+		efi_st_error("UninstallProtocolInterface failed\n");
+		return EFI_ST_FAILURE;
 	}
 
 	return EFI_ST_SUCCESS;
diff --git a/lib/efi_selftest/efi_selftest_miniapp_exit.c b/lib/efi_selftest/efi_selftest_miniapp_exit.c
new file mode 100644
index 0000000000..5ec57aba02
--- /dev/null
+++ b/lib/efi_selftest/efi_selftest_miniapp_exit.c
@@ -0,0 +1,37 @@
+/*
+ * efi_selftest_miniapp_exit
+ *
+ * Copyright (c) 2018 Heinrich Schuchardt
+ *
+ * SPDX-License-Identifier:     GPL-2.0+
+ *
+ * This EFI application is run by the StartImage selftest.
+ * It uses the Exit boot service to return.
+ */
+
+#include <common.h>
+#include <efi_api.h>
+
+/*
+ * 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;
+
+	con_out->output_string(con_out, L"EFI application calling Exit\n");
+
+	/* The return value is checked by the calling test */
+	systable->boottime->exit(handle, EFI_UNSUPPORTED, 0, NULL);
+
+	/*
+	 * 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_miniapp_return.c b/lib/efi_selftest/efi_selftest_miniapp_return.c
new file mode 100644
index 0000000000..0a82391df6
--- /dev/null
+++ b/lib/efi_selftest/efi_selftest_miniapp_return.c
@@ -0,0 +1,32 @@
+/*
+ * efi_selftest_miniapp_return
+ *
+ * Copyright (c) 2018 Heinrich Schuchardt
+ *
+ * SPDX-License-Identifier:     GPL-2.0+
+ *
+ * This EFI application is run by the StartImage selftest.
+ * It returns directly without calling the Exit boot service.
+ */
+
+#include <common.h>
+#include <efi_api.h>
+
+/*
+ * 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;
+
+	con_out->output_string(con_out,
+			       L"EFI application returning w/o calling Exit\n");
+
+	/* The return value is checked by the calling test */
+	return EFI_INCOMPATIBLE_VERSION;
+}
diff --git a/lib/efi_selftest/efi_selftest_startimage_exit.c b/lib/efi_selftest/efi_selftest_startimage_exit.c
new file mode 100644
index 0000000000..0809690e97
--- /dev/null
+++ b/lib/efi_selftest/efi_selftest_startimage_exit.c
@@ -0,0 +1,149 @@
+/*
+ * efi_selftest_start_image
+ *
+ * Copyright (c) 2018 Heinrich Schuchardt <xypron.glpk@gmx.de>
+ *
+ * SPDX-License-Identifier:     GPL-2.0+
+ *
+ * This test checks the StartImage boot service.
+ * The efi_selftest_miniapp_exit.efi application is loaded into memory
+ * and started.
+ */
+
+#include <efi_selftest.h>
+/* Include containing the miniapp.efi application */
+#include "efi_miniapp_file_image_exit.h"
+
+/* Block size of compressed disk image */
+#define COMPRESSED_DISK_IMAGE_BLOCK_SIZE 8
+
+/* Binary logarithm of the block size */
+#define LB_BLOCK_SIZE 9
+
+static efi_handle_t image_handle;
+static struct efi_boot_services *boottime;
+
+/* 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;
+
+/* Decompressed file image */
+static u8 *image;
+
+/*
+ * 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;
+}
+
+/*
+ * Setup unit test.
+ *
+ * @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)
+{
+	image_handle = handle;
+	boottime = systable->boottime;
+
+	/* Load the application image into memory */
+	decompress(&image);
+
+	return EFI_ST_SUCCESS;
+}
+
+/*
+ * Tear down unit test.
+ *
+ * @return:	EFI_ST_SUCCESS for success
+ */
+static int teardown(void)
+{
+	efi_status_t r = EFI_ST_SUCCESS;
+
+	if (image) {
+		r = efi_free_pool(image);
+		if (r != EFI_SUCCESS) {
+			efi_st_error("Failed to free image\n");
+			return EFI_ST_FAILURE;
+		}
+	}
+	return r;
+}
+
+/*
+ * Execute unit test.
+ *
+ * Load and start the application image.
+ *
+ * @return:	EFI_ST_SUCCESS for success
+ */
+static int execute(void)
+{
+	efi_status_t ret;
+	efi_handle_t handle;
+
+	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;
+	}
+	ret = boottime->start_image(handle, NULL, NULL);
+	if (ret != EFI_UNSUPPORTED) {
+		efi_st_error("Wrong return value from application\n");
+		return EFI_ST_FAILURE;
+	}
+
+	return EFI_ST_SUCCESS;
+}
+
+EFI_UNIT_TEST(startimage_exit) = {
+	.name = "start image exit",
+	.phase = EFI_EXECUTE_BEFORE_BOOTTIME_EXIT,
+	.setup = setup,
+	.execute = execute,
+	.teardown = teardown,
+};
diff --git a/lib/efi_selftest/efi_selftest_startimage_return.c b/lib/efi_selftest/efi_selftest_startimage_return.c
new file mode 100644
index 0000000000..2209911457
--- /dev/null
+++ b/lib/efi_selftest/efi_selftest_startimage_return.c
@@ -0,0 +1,149 @@
+/*
+ * efi_selftest_start_image
+ *
+ * Copyright (c) 2018 Heinrich Schuchardt <xypron.glpk@gmx.de>
+ *
+ * SPDX-License-Identifier:     GPL-2.0+
+ *
+ * This test checks the StartImage boot service.
+ * The efi_selftest_miniapp_return.efi application is loaded into memory
+ * and started.
+ */
+
+#include <efi_selftest.h>
+/* Include containing the miniapp.efi application */
+#include "efi_miniapp_file_image_return.h"
+
+/* Block size of compressed disk image */
+#define COMPRESSED_DISK_IMAGE_BLOCK_SIZE 8
+
+/* Binary logarithm of the block size */
+#define LB_BLOCK_SIZE 9
+
+static efi_handle_t image_handle;
+static struct efi_boot_services *boottime;
+
+/* 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;
+
+/* Decompressed file image */
+static u8 *image;
+
+/*
+ * 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;
+}
+
+/*
+ * Setup unit test.
+ *
+ * @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)
+{
+	image_handle = handle;
+	boottime = systable->boottime;
+
+	/* Load the application image into memory */
+	decompress(&image);
+
+	return EFI_ST_SUCCESS;
+}
+
+/*
+ * Tear down unit test.
+ *
+ * @return:	EFI_ST_SUCCESS for success
+ */
+static int teardown(void)
+{
+	efi_status_t r = EFI_ST_SUCCESS;
+
+	if (image) {
+		r = efi_free_pool(image);
+		if (r != EFI_SUCCESS) {
+			efi_st_error("Failed to free image\n");
+			return EFI_ST_FAILURE;
+		}
+	}
+	return r;
+}
+
+/*
+ * Execute unit test.
+ *
+ * Load and start the application image.
+ *
+ * @return:	EFI_ST_SUCCESS for success
+ */
+static int execute(void)
+{
+	efi_status_t ret;
+	efi_handle_t handle;
+
+	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;
+	}
+	ret = boottime->start_image(handle, NULL, NULL);
+	if (ret != EFI_INCOMPATIBLE_VERSION) {
+		efi_st_error("Wrong return value from application\n");
+		return EFI_ST_FAILURE;
+	}
+
+	return EFI_ST_SUCCESS;
+}
+
+EFI_UNIT_TEST(startimage) = {
+	.name = "start image return",
+	.phase = EFI_EXECUTE_BEFORE_BOOTTIME_EXIT,
+	.setup = setup,
+	.execute = execute,
+	.teardown = teardown,
+};
diff --git a/lib/efi_selftest/efi_selftest_tpl.c b/lib/efi_selftest/efi_selftest_tpl.c
index 6ea0bb7177..8243fae15b 100644
--- a/lib/efi_selftest/efi_selftest_tpl.c
+++ b/lib/efi_selftest/efi_selftest_tpl.c
@@ -144,9 +144,10 @@ static int execute(void)
 		efi_st_error("WaitForEvent returned wrong index\n");
 		return EFI_ST_FAILURE;
 	}
-	efi_st_printf("Notification count with TPL level TPL_APPLICATION: %u\n",
-		      notification_count);
 	if (notification_count < 8 || notification_count > 12) {
+		efi_st_printf(
+		    "Notification count with TPL level TPL_APPLICATION: %u\n",
+		    notification_count);
 		efi_st_error("Incorrect timing of events\n");
 		return EFI_ST_FAILURE;
 	}
@@ -181,9 +182,10 @@ static int execute(void)
 		efi_st_error("Could not check event\n");
 		return EFI_ST_FAILURE;
 	}
-	efi_st_printf("Notification count with TPL level TPL_CALLBACK: %u\n",
-		      notification_count);
 	if (notification_count != 0) {
+		efi_st_printf(
+			"Notification count with TPL level TPL_CALLBACK: %u\n",
+			notification_count);
 		efi_st_error("Suppressed timer fired\n");
 		return EFI_ST_FAILURE;
 	}
@@ -200,9 +202,10 @@ static int execute(void)
 		efi_st_error("Could not wait for event\n");
 		return EFI_ST_FAILURE;
 	}
-	efi_st_printf("Notification count with TPL level TPL_APPLICATION: %u\n",
-		      notification_count);
 	if (notification_count < 1) {
+		efi_st_printf(
+		    "Notification count with TPL level TPL_APPLICATION: %u\n",
+		    notification_count);
 		efi_st_error("Queued timer event did not fire\n");
 		return EFI_ST_FAILURE;
 	}
diff --git a/lib/vsprintf.c b/lib/vsprintf.c
index dd572d2868..226f4eb3e5 100644
--- a/lib/vsprintf.c
+++ b/lib/vsprintf.c
@@ -11,16 +11,17 @@
  * from hush: simple_itoa() was lifted from boa-0.93.15
  */
 
-#include <stdarg.h>
-#include <linux/types.h>
-#include <linux/string.h>
-#include <linux/ctype.h>
-
 #include <common.h>
 #include <charset.h>
+#include <efi_loader.h>
+#include <div64.h>
 #include <uuid.h>
+#include <stdarg.h>
+#include <linux/ctype.h>
+#include <linux/err.h>
+#include <linux/types.h>
+#include <linux/string.h>
 
-#include <div64.h>
 #define noinline __attribute__((noinline))
 
 /* we use this so that we can do without the ctype library */
@@ -292,6 +293,26 @@ static char *string16(char *buf, char *end, u16 *s, int field_width,
 	return buf;
 }
 
+#if defined(CONFIG_EFI_LOADER) && \
+	!defined(CONFIG_SPL_BUILD) && !defined(API_BUILD)
+static char *device_path_string(char *buf, char *end, void *dp, int field_width,
+				int precision, int flags)
+{
+	u16 *str;
+
+	if (!dp)
+		return "<NULL>";
+
+	str = efi_dp_str((struct efi_device_path *)dp);
+	if (!str)
+		return ERR_PTR(-ENOMEM);
+
+	buf = string16(buf, end, str, field_width, precision, flags);
+	efi_free_pool(str);
+	return buf;
+}
+#endif
+
 #ifdef CONFIG_CMD_NET
 static const char hex_asc[] = "0123456789abcdef";
 #define hex_asc_lo(x)	hex_asc[((x) & 0x0f)]
@@ -435,6 +456,12 @@ static char *pointer(const char *fmt, char *buf, char *end, void *ptr,
 #endif
 
 	switch (*fmt) {
+#if defined(CONFIG_EFI_LOADER) && \
+	!defined(CONFIG_SPL_BUILD) && !defined(API_BUILD)
+	case 'D':
+		return device_path_string(buf, end, ptr, field_width,
+					  precision, flags);
+#endif
 #ifdef CONFIG_CMD_NET
 	case 'a':
 		flags |= SPECIAL | ZEROPAD;
@@ -604,6 +631,8 @@ repeat:
 			str = pointer(fmt + 1, str, end,
 					va_arg(args, void *),
 					field_width, precision, flags);
+			if (IS_ERR(str))
+				return PTR_ERR(str);
 			/* Skip all alphanumeric pointer suffixes */
 			while (isalnum(fmt[1]))
 				fmt++;
@@ -768,6 +797,9 @@ int printf(const char *fmt, ...)
 	i = vscnprintf(printbuffer, sizeof(printbuffer), fmt, args);
 	va_end(args);
 
+	/* Handle error */
+	if (i <= 0)
+		return i;
 	/* Print the string */
 	puts(printbuffer);
 	return i;
@@ -784,6 +816,9 @@ int vprintf(const char *fmt, va_list args)
 	 */
 	i = vscnprintf(printbuffer, sizeof(printbuffer), fmt, args);
 
+	/* Handle error */
+	if (i <= 0)
+		return i;
 	/* Print the string */
 	puts(printbuffer);
 	return i;
diff --git a/test/print_ut.c b/test/print_ut.c
index a42c554bef..1aa68be7a9 100644
--- a/test/print_ut.c
+++ b/test/print_ut.c
@@ -7,12 +7,46 @@
 #define DEBUG
 
 #include <common.h>
+#if defined(CONFIG_EFI_LOADER) && \
+	!defined(CONFIG_SPL_BUILD) && !defined(API_BUILD)
+#include <efi_api.h>
+#endif
 #include <display_options.h>
 #include <version.h>
 
 #define FAKE_BUILD_TAG	"jenkins-u-boot-denx_uboot_dm-master-build-aarch64" \
 			"and a lot more text to come"
 
+/* Test efi_loader specific printing */
+static void efi_ut_print(void)
+{
+#if defined(CONFIG_EFI_LOADER) && \
+    !defined(CONFIG_SPL_BUILD) && !defined(API_BUILD)
+	char str[10];
+	u8 buf[sizeof(struct efi_device_path_sd_mmc_path) +
+	       sizeof(struct efi_device_path)];
+	u8 *pos = buf;
+	struct efi_device_path *dp_end;
+	struct efi_device_path_sd_mmc_path *dp_sd =
+			(struct efi_device_path_sd_mmc_path *)pos;
+
+	/* Create a device path for an SD card */
+	dp_sd->dp.type = DEVICE_PATH_TYPE_MESSAGING_DEVICE;
+	dp_sd->dp.sub_type = DEVICE_PATH_SUB_TYPE_MSG_SD;
+	dp_sd->dp.length = sizeof(struct efi_device_path_sd_mmc_path);
+	dp_sd->slot_number = 3;
+	pos += sizeof(struct efi_device_path_sd_mmc_path);
+	/* Append end node */
+	dp_end = (struct efi_device_path *)pos;
+	dp_end->type = DEVICE_PATH_TYPE_END;
+	dp_end->sub_type = DEVICE_PATH_SUB_TYPE_END;
+	dp_end->length = sizeof(struct efi_device_path);
+
+	snprintf(str, sizeof(str), "_%pD_", buf);
+	assert(!strcmp("_/SD(3)_", str));
+#endif
+}
+
 static int do_ut_print(cmd_tbl_t *cmdtp, int flag, int argc,
 		       char *const argv[])
 {
@@ -75,6 +109,9 @@ static int do_ut_print(cmd_tbl_t *cmdtp, int flag, int argc,
 	assert(!strncmp(FAKE_BUILD_TAG, s + 9 + len, 12));
 	assert(!strcmp("\n\n", s + big_str_len - 3));
 
+	/* Test efi_loader specific printing */
+	efi_ut_print();
+
 	printf("%s: Everything went swimmingly\n", __func__);
 	return 0;
 }
diff --git a/tools/.gitignore b/tools/.gitignore
index 6a487d2202..c8cdaef90c 100644
--- a/tools/.gitignore
+++ b/tools/.gitignore
@@ -6,6 +6,7 @@
 /easylogo/easylogo
 /envcrc
 /fdtgrep
+/file2include
 /fit_check_sign
 /fit_info
 /gdb/gdbcont
diff --git a/tools/Makefile b/tools/Makefile
index 571f571ec9..b7d7d418ee 100644
--- a/tools/Makefile
+++ b/tools/Makefile
@@ -57,6 +57,8 @@ mkenvimage-objs := mkenvimage.o os_support.o lib/crc32.o
 hostprogs-y += dumpimage mkimage
 hostprogs-$(CONFIG_FIT_SIGNATURE) += fit_info fit_check_sign
 
+hostprogs-$(CONFIG_CMD_BOOTEFI_SELFTEST) += file2include
+
 FIT_SIG_OBJS-$(CONFIG_FIT_SIGNATURE) := common/image-sig.o
 
 # The following files are synced with upstream DTC.
@@ -118,6 +120,7 @@ dumpimage-objs := $(dumpimage-mkimage-objs) dumpimage.o
 mkimage-objs   := $(dumpimage-mkimage-objs) mkimage.o
 fit_info-objs   := $(dumpimage-mkimage-objs) fit_info.o
 fit_check_sign-objs   := $(dumpimage-mkimage-objs) fit_check_sign.o
+file2include-objs := file2include.o
 
 ifneq ($(CONFIG_MX23)$(CONFIG_MX28),)
 # Add CONFIG_MXS into host CFLAGS, so we can check whether or not register
diff --git a/tools/file2include.c b/tools/file2include.c
new file mode 100644
index 0000000000..9145f0845a
--- /dev/null
+++ b/tools/file2include.c
@@ -0,0 +1,106 @@
+/*
+ * Convert a file image to a C define
+ *
+ * Copyright (c) 2017 Heinrich Schuchardt <xypron.glpk@gmx.de>
+ *
+ * SPDX-License-Identifier:     GPL-2.0+
+ *
+ * For testing EFI disk management we need an in memory image of
+ * a disk.
+ *
+ * The tool file2include converts a file to a C include. The file
+ * is separated into strings of 8 bytes. Only the non-zero strings
+ * are written to the include. The output format has been designed
+ * to maintain readability.
+ *
+ * As the disk image needed for testing contains mostly zeroes a high
+ * compression ratio can be attained.
+ */
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <malloc.h>
+
+/* Size of the blocks written to the compressed file */
+#define BLOCK_SIZE 8
+
+int main(int argc, char *argv[])
+{
+	FILE *file;
+	int ret;
+	unsigned char *buf;
+	size_t count, i, j;
+
+	/* Provide usage help */
+	if (argc != 2) {
+		printf("Usage:\n%s FILENAME\n", argv[0]);
+		return EXIT_FAILURE;
+	}
+	/* Open file */
+	file = fopen(argv[1], "r");
+	if (!file) {
+		perror("fopen");
+		return EXIT_FAILURE;
+	}
+	/* Get file length */
+	ret = fseek(file, 0, SEEK_END);
+	if (ret < 0) {
+		perror("fseek");
+		return EXIT_FAILURE;
+	}
+	count = ftell(file);
+	if (!count) {
+		fprintf(stderr, "File %s has length 0\n", argv[1]);
+		return EXIT_FAILURE;
+	}
+	rewind(file);
+	/* Read file */
+	buf = malloc(count);
+	if (!buf) {
+		perror("calloc");
+		return EXIT_FAILURE;
+	}
+	count = fread(buf, 1, count, file);
+
+	/* Generate output */
+	printf("/*\n");
+	printf(" *  Non-zero %u byte strings of a disk image\n", BLOCK_SIZE);
+	printf(" *\n");
+	printf(" *  Generated with tools/file2include\n");
+	printf(" *\n");
+	printf(" *  SPDX-License-Identifier:	GPL-2.0+\n");
+	printf(" */\n\n");
+	printf("#define EFI_ST_DISK_IMG { 0x%08zx, { \\\n", count);
+
+	for (i = 0; i < count; i += BLOCK_SIZE) {
+		int c = 0;
+
+		for (j = i; j < i + BLOCK_SIZE && j < count; ++j) {
+			if (buf[j])
+				c = 1;
+		}
+		if (!c)
+			continue;
+		printf("\t{0x%08zx, \"", i);
+		for (j = i; j < i + BLOCK_SIZE && j < count; ++j)
+			printf("\\x%02x", buf[j]);
+		printf("\"}, /* ");
+		for (j = i; j < i + BLOCK_SIZE && j < count; ++j) {
+			if (buf[j] >= 0x20 && buf[j] <= 0x7e)
+				printf("%c", buf[j]);
+			else
+				printf(".");
+		}
+		printf(" */ \\\n");
+	}
+	printf("\t{0, NULL} } }\n");
+
+	/* Release resources */
+	free(buf);
+	ret = fclose(file);
+	if (ret) {
+		perror("fclose");
+		return EXIT_FAILURE;
+	}
+	return EXIT_SUCCESS;
+}