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

Patch queue for efi - 2017-12-05

Highlights for this release:

  - Dynamic EFI object creation (lists instead of static arrays)
  - EFI selftest improvements
  - Minor fixes

25 files changed, 2206 insertions, 597 deletions

diff --git a/lib/charset.c b/lib/charset.c
index ff76e88..8cd17ea 100644
--- a/lib/charset.c
+++ b/lib/charset.c
@@ -6,7 +6,6 @@
  *  SPDX-License-Identifier:     GPL-2.0+
  */
 
-#include <common.h>
 #include <charset.h>
 #include <malloc.h>
 
@@ -99,3 +98,59 @@ uint8_t *utf16_to_utf8(uint8_t *dest, const uint16_t *src, size_t size)
 
 	return dest;
 }
+
+uint16_t *utf8_to_utf16(uint16_t *dest, const uint8_t *src, size_t size)
+{
+	while (size--) {
+		int extension_bytes;
+		uint32_t code;
+
+		extension_bytes = 0;
+		if (*src <= 0x7f) {
+			code = *src++;
+			/* Exit on zero byte */
+			if (!code)
+				size = 0;
+		} else if (*src <= 0xbf) {
+			/* Illegal code */
+			code = '?';
+		} else if (*src <= 0xdf) {
+			code = *src++ & 0x1f;
+			extension_bytes = 1;
+		} else if (*src <= 0xef) {
+			code = *src++ & 0x0f;
+			extension_bytes = 2;
+		} else if (*src <= 0xf7) {
+			code = *src++ & 0x07;
+			extension_bytes = 3;
+		} else {
+			/* Illegal code */
+			code = '?';
+		}
+
+		for (; extension_bytes && size; --size, --extension_bytes) {
+			if ((*src & 0xc0) == 0x80) {
+				code <<= 6;
+				code |= *src++ & 0x3f;
+			} else {
+				/* Illegal code */
+				code = '?';
+				++src;
+				--size;
+				break;
+			}
+		}
+
+		if (code < 0x10000) {
+			*dest++ = code;
+		} else {
+			/*
+			 * Simplified expression for
+			 * (((code - 0x10000) >> 10) & 0x3ff) | 0xd800
+			 */
+			*dest++ = (code >> 10) + 0xd7c0;
+			*dest++ = (code & 0x3ff) | 0xdc00;
+		}
+	}
+	return dest;
+}
diff --git a/lib/efi/efi_stub.c b/lib/efi/efi_stub.c
index 1814960..2e8d409 100644
--- a/lib/efi/efi_stub.c
+++ b/lib/efi/efi_stub.c
@@ -277,7 +277,7 @@ efi_status_t efi_main(efi_handle_t image, struct efi_system_table *sys_table)
 	struct efi_boot_services *boot = sys_table->boottime;
 	struct efi_mem_desc *desc;
 	struct efi_entry_memmap map;
-	ulong key, desc_size, size;
+	efi_uintn_t key, desc_size, size;
 	efi_status_t ret;
 	u32 version;
 	int cs32;
diff --git a/lib/efi_loader/Makefile b/lib/efi_loader/Makefile
index ddb978f..2722265 100644
--- a/lib/efi_loader/Makefile
+++ b/lib/efi_loader/Makefile
@@ -7,8 +7,8 @@
 # This file only gets included with CONFIG_EFI_LOADER set, so all
 # object inclusion implicitly depends on it
 
-CFLAGS_helloworld.o := $(CFLAGS_EFI)
-CFLAGS_REMOVE_helloworld.o := $(CFLAGS_NON_EFI)
+CFLAGS_helloworld.o := $(CFLAGS_EFI) -Os -ffreestanding
+CFLAGS_REMOVE_helloworld.o := $(CFLAGS_NON_EFI) -Os
 
 ifneq ($(CONFIG_CMD_BOOTEFI_HELLO_COMPILE),)
 always += helloworld.efi
@@ -17,7 +17,7 @@ endif
 obj-$(CONFIG_CMD_BOOTEFI_HELLO) += helloworld_efi.o
 obj-y += efi_image_loader.o efi_boottime.o efi_runtime.o efi_console.o
 obj-y += efi_memory.o efi_device_path_to_text.o efi_device_path.o
-obj-y += efi_file.o efi_variable.o efi_bootmgr.o
+obj-y += efi_file.o efi_variable.o efi_bootmgr.o efi_watchdog.o
 obj-$(CONFIG_LCD) += efi_gop.o
 obj-$(CONFIG_DM_VIDEO) += efi_gop.o
 obj-$(CONFIG_PARTITIONS) += efi_disk.o
diff --git a/lib/efi_loader/efi_boottime.c b/lib/efi_loader/efi_boottime.c
index 743b848..a37fb25 100644
--- a/lib/efi_loader/efi_boottime.c
+++ b/lib/efi_loader/efi_boottime.c
@@ -21,7 +21,7 @@
 DECLARE_GLOBAL_DATA_PTR;
 
 /* Task priority level */
-static UINTN efi_tpl = TPL_APPLICATION;
+static efi_uintn_t efi_tpl = TPL_APPLICATION;
 
 /* This list contains all the EFI objects our payload has access to */
 LIST_HEAD(efi_obj_list);
@@ -156,18 +156,6 @@ void efi_signal_event(struct efi_event *event)
 }
 
 /*
- * Write a debug message for an EPI API service that is not implemented yet.
- *
- * @funcname	function that is not yet implemented
- * @return	EFI_UNSUPPORTED
- */
-static efi_status_t efi_unsupported(const char *funcname)
-{
-	debug("EFI: App called into unimplemented function %s\n", funcname);
-	return EFI_EXIT(EFI_UNSUPPORTED);
-}
-
-/*
  * Raise the task priority level.
  *
  * This function implements the RaiseTpl service.
@@ -177,9 +165,9 @@ static efi_status_t efi_unsupported(const char *funcname)
  * @new_tpl	new value of the task priority level
  * @return	old value of the task priority level
  */
-static unsigned long EFIAPI efi_raise_tpl(UINTN new_tpl)
+static unsigned long EFIAPI efi_raise_tpl(efi_uintn_t new_tpl)
 {
-	UINTN old_tpl = efi_tpl;
+	efi_uintn_t old_tpl = efi_tpl;
 
 	EFI_ENTRY("0x%zx", new_tpl);
 
@@ -202,7 +190,7 @@ static unsigned long EFIAPI efi_raise_tpl(UINTN new_tpl)
  *
  * @old_tpl	value of the task priority level to be restored
  */
-static void EFIAPI efi_restore_tpl(UINTN old_tpl)
+static void EFIAPI efi_restore_tpl(efi_uintn_t old_tpl)
 {
 	EFI_ENTRY("0x%zx", old_tpl);
 
@@ -229,12 +217,12 @@ static void EFIAPI efi_restore_tpl(UINTN old_tpl)
  * @return		status code
  */
 static efi_status_t EFIAPI efi_allocate_pages_ext(int type, int memory_type,
-						  unsigned long pages,
+						  efi_uintn_t pages,
 						  uint64_t *memory)
 {
 	efi_status_t r;
 
-	EFI_ENTRY("%d, %d, 0x%lx, %p", type, memory_type, pages, memory);
+	EFI_ENTRY("%d, %d, 0x%zx, %p", type, memory_type, pages, memory);
 	r = efi_allocate_pages(type, memory_type, pages, memory);
 	return EFI_EXIT(r);
 }
@@ -251,11 +239,11 @@ static efi_status_t EFIAPI efi_allocate_pages_ext(int type, int memory_type,
  * @return	status code
  */
 static efi_status_t EFIAPI efi_free_pages_ext(uint64_t memory,
-					      unsigned long pages)
+					      efi_uintn_t pages)
 {
 	efi_status_t r;
 
-	EFI_ENTRY("%"PRIx64", 0x%lx", memory, pages);
+	EFI_ENTRY("%"PRIx64", 0x%zx", memory, pages);
 	r = efi_free_pages(memory, pages);
 	return EFI_EXIT(r);
 }
@@ -276,10 +264,10 @@ static efi_status_t EFIAPI efi_free_pages_ext(uint64_t memory,
  * @return		status code
  */
 static efi_status_t EFIAPI efi_get_memory_map_ext(
-					unsigned long *memory_map_size,
+					efi_uintn_t *memory_map_size,
 					struct efi_mem_desc *memory_map,
-					unsigned long *map_key,
-					unsigned long *descriptor_size,
+					efi_uintn_t *map_key,
+					efi_uintn_t *descriptor_size,
 					uint32_t *descriptor_version)
 {
 	efi_status_t r;
@@ -304,12 +292,12 @@ static efi_status_t EFIAPI efi_get_memory_map_ext(
  * @return	status code
  */
 static efi_status_t EFIAPI efi_allocate_pool_ext(int pool_type,
-						 unsigned long size,
+						 efi_uintn_t size,
 						 void **buffer)
 {
 	efi_status_t r;
 
-	EFI_ENTRY("%d, %ld, %p", pool_type, size, buffer);
+	EFI_ENTRY("%d, %zd, %p", pool_type, size, buffer);
 	r = efi_allocate_pool(pool_type, size, buffer);
 	return EFI_EXIT(r);
 }
@@ -333,7 +321,30 @@ static efi_status_t EFIAPI efi_free_pool_ext(void *buffer)
 	return EFI_EXIT(r);
 }
 
-static efi_status_t efi_create_handle(void **handle)
+/*
+ * Add a new object to the object list.
+ *
+ * The protocols list is initialized.
+ * The object handle is set.
+ *
+ * @obj	object to be added
+ */
+void efi_add_handle(struct efi_object *obj)
+{
+	if (!obj)
+		return;
+	INIT_LIST_HEAD(&obj->protocols);
+	obj->handle = obj;
+	list_add_tail(&obj->link, &efi_obj_list);
+}
+
+/*
+ * Create handle.
+ *
+ * @handle	new handle
+ * @return	status code
+ */
+efi_status_t efi_create_handle(void **handle)
 {
 	struct efi_object *obj;
 	efi_status_t r;
@@ -343,10 +354,8 @@ static efi_status_t efi_create_handle(void **handle)
 			      (void **)&obj);
 	if (r != EFI_SUCCESS)
 		return r;
-	memset(obj, 0, sizeof(struct efi_object));
-	obj->handle = obj;
-	list_add_tail(&obj->link, &efi_obj_list);
-	*handle = obj;
+	efi_add_handle(obj);
+	*handle = obj->handle;
 	return r;
 }
 
@@ -371,7 +380,7 @@ static struct efi_event efi_events[16];
  * @event		created event
  * @return		status code
  */
-efi_status_t efi_create_event(uint32_t type, UINTN notify_tpl,
+efi_status_t efi_create_event(uint32_t type, efi_uintn_t notify_tpl,
 			      void (EFIAPI *notify_function) (
 					struct efi_event *event,
 					void *context),
@@ -421,7 +430,7 @@ efi_status_t efi_create_event(uint32_t type, UINTN notify_tpl,
  * @return		status code
  */
 static efi_status_t EFIAPI efi_create_event_ext(
-			uint32_t type, UINTN notify_tpl,
+			uint32_t type, efi_uintn_t notify_tpl,
 			void (EFIAPI *notify_function) (
 					struct efi_event *event,
 					void *context),
@@ -551,13 +560,13 @@ static efi_status_t EFIAPI efi_set_timer_ext(struct efi_event *event,
  * @index	index of the event that was signaled
  * @return	status code
  */
-static efi_status_t EFIAPI efi_wait_for_event(unsigned long num_events,
+static efi_status_t EFIAPI efi_wait_for_event(efi_uintn_t num_events,
 					      struct efi_event **event,
-					      size_t *index)
+					      efi_uintn_t *index)
 {
 	int i, j;
 
-	EFI_ENTRY("%ld, %p, %p", num_events, event, index);
+	EFI_ENTRY("%zd, %p, %p", num_events, event, index);
 
 	/* Check parameters */
 	if (!num_events || !event)
@@ -691,75 +700,137 @@ static efi_status_t EFIAPI efi_check_event(struct efi_event *event)
 }
 
 /*
- * Install protocol interface.
+ * Find the internal EFI object for a handle.
+ *
+ * @handle	handle to find
+ * @return	EFI object
+ */
+struct efi_object *efi_search_obj(const void *handle)
+{
+	struct efi_object *efiobj;
+
+	list_for_each_entry(efiobj, &efi_obj_list, link) {
+		if (efiobj->handle == handle)
+			return efiobj;
+	}
+
+	return NULL;
+}
+
+/*
+ * Find a protocol on a handle.
  *
- * This is the function for internal calls. For the API implementation of the
- * InstallProtocolInterface service see function
- * efi_install_protocol_interface_ext.
+ * @handle		handle
+ * @protocol_guid	GUID of the protocol
+ * @handler		reference to the protocol
+ * @return		status code
+ */
+efi_status_t efi_search_protocol(const void *handle,
+				 const efi_guid_t *protocol_guid,
+				 struct efi_handler **handler)
+{
+	struct efi_object *efiobj;
+	struct list_head *lhandle;
+
+	if (!handle || !protocol_guid)
+		return EFI_INVALID_PARAMETER;
+	efiobj = efi_search_obj(handle);
+	if (!efiobj)
+		return EFI_INVALID_PARAMETER;
+	list_for_each(lhandle, &efiobj->protocols) {
+		struct efi_handler *protocol;
+
+		protocol = list_entry(lhandle, struct efi_handler, link);
+		if (!guidcmp(protocol->guid, protocol_guid)) {
+			if (handler)
+				*handler = protocol;
+			return EFI_SUCCESS;
+		}
+	}
+	return EFI_NOT_FOUND;
+}
+
+/*
+ * Install new protocol on a handle.
  *
  * @handle			handle on which the protocol shall be installed
  * @protocol			GUID of the protocol to be installed
- * @protocol_interface_type	type of the interface to be installed,
- *				always EFI_NATIVE_INTERFACE
  * @protocol_interface		interface of the protocol implementation
  * @return			status code
  */
-static efi_status_t EFIAPI efi_install_protocol_interface(void **handle,
-			const efi_guid_t *protocol, int protocol_interface_type,
-			void *protocol_interface)
+efi_status_t efi_add_protocol(const void *handle, const efi_guid_t *protocol,
+			      void *protocol_interface)
 {
-	struct list_head *lhandle;
-	int i;
-	efi_status_t r;
+	struct efi_object *efiobj;
+	struct efi_handler *handler;
+	efi_status_t ret;
 
-	if (!handle || !protocol ||
-	    protocol_interface_type != EFI_NATIVE_INTERFACE) {
-		r = EFI_INVALID_PARAMETER;
-		goto out;
-	}
+	efiobj = efi_search_obj(handle);
+	if (!efiobj)
+		return EFI_INVALID_PARAMETER;
+	ret = efi_search_protocol(handle, protocol, NULL);
+	if (ret != EFI_NOT_FOUND)
+		return EFI_INVALID_PARAMETER;
+	handler = calloc(1, sizeof(struct efi_handler));
+	if (!handler)
+		return EFI_OUT_OF_RESOURCES;
+	handler->guid = protocol;
+	handler->protocol_interface = protocol_interface;
+	list_add_tail(&handler->link, &efiobj->protocols);
+	return EFI_SUCCESS;
+}
 
-	/* Create new handle if requested. */
-	if (!*handle) {
-		r = efi_create_handle(handle);
-		if (r != EFI_SUCCESS)
-			goto out;
-	}
-	/* Find object. */
-	list_for_each(lhandle, &efi_obj_list) {
-		struct efi_object *efiobj;
-		efiobj = list_entry(lhandle, struct efi_object, link);
+/*
+ * Delete protocol from a handle.
+ *
+ * @handle			handle from which the protocol shall be deleted
+ * @protocol			GUID of the protocol to be deleted
+ * @protocol_interface		interface of the protocol implementation
+ * @return			status code
+ */
+efi_status_t efi_remove_protocol(const void *handle, const efi_guid_t *protocol,
+				 void *protocol_interface)
+{
+	struct efi_handler *handler;
+	efi_status_t ret;
 
-		if (efiobj->handle != *handle)
-			continue;
-		/* Check if protocol is already installed on the handle. */
-		for (i = 0; i < ARRAY_SIZE(efiobj->protocols); i++) {
-			struct efi_handler *handler = &efiobj->protocols[i];
-
-			if (!handler->guid)
-				continue;
-			if (!guidcmp(handler->guid, protocol)) {
-				r = EFI_INVALID_PARAMETER;
-				goto out;
-			}
-		}
-		/* Install protocol in first empty slot. */
-		for (i = 0; i < ARRAY_SIZE(efiobj->protocols); i++) {
-			struct efi_handler *handler = &efiobj->protocols[i];
+	ret = efi_search_protocol(handle, protocol, &handler);
+	if (ret != EFI_SUCCESS)
+		return ret;
+	if (guidcmp(handler->guid, protocol))
+		return EFI_INVALID_PARAMETER;
+	list_del(&handler->link);
+	free(handler);
+	return EFI_SUCCESS;
+}
 
-			if (handler->guid)
-				continue;
+/*
+ * Delete all protocols from a handle.
+ *
+ * @handle			handle from which the protocols shall be deleted
+ * @return			status code
+ */
+efi_status_t efi_remove_all_protocols(const void *handle)
+{
+	struct efi_object *efiobj;
+	struct list_head *lhandle;
+	struct list_head *pos;
 
-			handler->guid = protocol;
-			handler->protocol_interface = protocol_interface;
-			r = EFI_SUCCESS;
-			goto out;
-		}
-		r = EFI_OUT_OF_RESOURCES;
-		goto out;
+	efiobj = efi_search_obj(handle);
+	if (!efiobj)
+		return EFI_INVALID_PARAMETER;
+	list_for_each_safe(lhandle, pos, &efiobj->protocols) {
+		struct efi_handler *protocol;
+		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)
+			return ret;
 	}
-	r = EFI_INVALID_PARAMETER;
-out:
-	return r;
+	return EFI_SUCCESS;
 }
 
 /*
@@ -776,16 +847,36 @@ out:
  * @protocol_interface		interface of the protocol implementation
  * @return			status code
  */
-static efi_status_t EFIAPI efi_install_protocol_interface_ext(void **handle,
-			const efi_guid_t *protocol, int protocol_interface_type,
-			void *protocol_interface)
+static efi_status_t EFIAPI efi_install_protocol_interface(
+			void **handle, const efi_guid_t *protocol,
+			int protocol_interface_type, void *protocol_interface)
 {
+	efi_status_t r;
+
 	EFI_ENTRY("%p, %pUl, %d, %p", handle, protocol, protocol_interface_type,
 		  protocol_interface);
 
-	return EFI_EXIT(efi_install_protocol_interface(handle, protocol,
-						       protocol_interface_type,
-						       protocol_interface));
+	if (!handle || !protocol ||
+	    protocol_interface_type != EFI_NATIVE_INTERFACE) {
+		r = EFI_INVALID_PARAMETER;
+		goto out;
+	}
+
+	/* Create new handle if requested. */
+	if (!*handle) {
+		r = efi_create_handle(handle);
+		if (r != EFI_SUCCESS)
+			goto out;
+		debug("%sEFI: new handle %p\n", indent_string(nesting_level),
+		      *handle);
+	} else {
+		debug("%sEFI: handle %p\n", indent_string(nesting_level),
+		      *handle);
+	}
+	/* Add new protocol */
+	r = efi_add_protocol(*handle, protocol, protocol_interface);
+out:
+	return EFI_EXIT(r);
 }
 
 /*
@@ -814,75 +905,41 @@ static efi_status_t EFIAPI efi_reinstall_protocol_interface(void *handle,
 /*
  * Uninstall protocol interface.
  *
- * This is the function for internal calls. For the API implementation of the
- * UninstallProtocolInterface service see function
- * efi_uninstall_protocol_interface_ext.
+ * This function implements the UninstallProtocolInterface service.
+ * See the Unified Extensible Firmware Interface (UEFI) specification
+ * for details.
  *
  * @handle			handle from which the protocol shall be removed
  * @protocol			GUID of the protocol to be removed
  * @protocol_interface		interface to be removed
  * @return			status code
  */
-static efi_status_t EFIAPI efi_uninstall_protocol_interface(void *handle,
-			const efi_guid_t *protocol, void *protocol_interface)
+static efi_status_t EFIAPI efi_uninstall_protocol_interface(
+				void *handle, const efi_guid_t *protocol,
+				void *protocol_interface)
 {
-	struct list_head *lhandle;
-	int i;
-	efi_status_t r = EFI_NOT_FOUND;
+	struct efi_handler *handler;
+	efi_status_t r;
+
+	EFI_ENTRY("%p, %pUl, %p", handle, protocol, protocol_interface);
 
 	if (!handle || !protocol) {
 		r = EFI_INVALID_PARAMETER;
 		goto out;
 	}
 
-	list_for_each(lhandle, &efi_obj_list) {
-		struct efi_object *efiobj;
-		efiobj = list_entry(lhandle, struct efi_object, link);
-
-		if (efiobj->handle != handle)
-			continue;
-
-		for (i = 0; i < ARRAY_SIZE(efiobj->protocols); i++) {
-			struct efi_handler *handler = &efiobj->protocols[i];
-			const efi_guid_t *hprotocol = handler->guid;
-
-			if (!hprotocol)
-				continue;
-			if (!guidcmp(hprotocol, protocol)) {
-				if (handler->protocol_interface) {
-					r = EFI_ACCESS_DENIED;
-				} else {
-					handler->guid = 0;
-					r = EFI_SUCCESS;
-				}
-				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 */
+		r =  EFI_ACCESS_DENIED;
+	} else {
+		r = efi_remove_protocol(handle, protocol, protocol_interface);
 	}
-
 out:
-	return r;
-}
-
-/*
- * Uninstall protocol interface.
- *
- * This function implements the UninstallProtocolInterface service.
- * See the Unified Extensible Firmware Interface (UEFI) specification
- * for details.
- *
- * @handle			handle from which the protocol shall be removed
- * @protocol			GUID of the protocol to be removed
- * @protocol_interface		interface to be removed
- * @return			status code
- */
-static efi_status_t EFIAPI efi_uninstall_protocol_interface_ext(void *handle,
-			const efi_guid_t *protocol, void *protocol_interface)
-{
-	EFI_ENTRY("%p, %pUl, %p", handle, protocol, protocol_interface);
-
-	return EFI_EXIT(efi_uninstall_protocol_interface(handle, protocol,
-							 protocol_interface));
+	return EFI_EXIT(r);
 }
 
 /*
@@ -922,23 +979,21 @@ static int efi_search(enum efi_locate_search_type search_type,
 		      const efi_guid_t *protocol, void *search_key,
 		      struct efi_object *efiobj)
 {
-	int i;
+	efi_status_t ret;
 
 	switch (search_type) {
-	case all_handles:
+	case ALL_HANDLES:
 		return 0;
-	case by_register_notify:
+	case BY_REGISTER_NOTIFY:
+		/* TODO: RegisterProtocolNotify is not implemented yet */
 		return -1;
-	case by_protocol:
-		for (i = 0; i < ARRAY_SIZE(efiobj->protocols); i++) {
-			const efi_guid_t *guid = efiobj->protocols[i].guid;
-			if (guid && !guidcmp(guid, protocol))
-				return 0;
-		}
+	case BY_PROTOCOL:
+		ret = efi_search_protocol(efiobj->handle, protocol, NULL);
+		return (ret != EFI_SUCCESS);
+	default:
+		/* Invalid search type */
 		return -1;
 	}
-
-	return -1;
 }
 
 /*
@@ -957,18 +1012,40 @@ static int efi_search(enum efi_locate_search_type search_type,
 static efi_status_t efi_locate_handle(
 			enum efi_locate_search_type search_type,
 			const efi_guid_t *protocol, void *search_key,
-			unsigned long *buffer_size, efi_handle_t *buffer)
+			efi_uintn_t *buffer_size, efi_handle_t *buffer)
 {
-	struct list_head *lhandle;
-	unsigned long size = 0;
+	struct efi_object *efiobj;
+	efi_uintn_t size = 0;
+
+	/* Check parameters */
+	switch (search_type) {
+	case ALL_HANDLES:
+		break;
+	case BY_REGISTER_NOTIFY:
+		if (!search_key)
+			return EFI_INVALID_PARAMETER;
+		/* RegisterProtocolNotify is not implemented yet */
+		return EFI_UNSUPPORTED;
+	case BY_PROTOCOL:
+		if (!protocol)
+			return EFI_INVALID_PARAMETER;
+		break;
+	default:
+		return EFI_INVALID_PARAMETER;
+	}
+
+	/*
+	 * efi_locate_handle_buffer uses this function for
+	 * the calculation of the necessary buffer size.
+	 * So do not require a buffer for buffersize == 0.
+	 */
+	if (!buffer_size || (*buffer_size && !buffer))
+		return EFI_INVALID_PARAMETER;
 
 	/* Count how much space we need */
-	list_for_each(lhandle, &efi_obj_list) {
-		struct efi_object *efiobj;
-		efiobj = list_entry(lhandle, struct efi_object, link);
-		if (!efi_search(search_type, protocol, search_key, efiobj)) {
+	list_for_each_entry(efiobj, &efi_obj_list, link) {
+		if (!efi_search(search_type, protocol, search_key, efiobj))
 			size += sizeof(void*);
-		}
 	}
 
 	if (*buffer_size < size) {
@@ -981,12 +1058,9 @@ static efi_status_t efi_locate_handle(
 		return EFI_NOT_FOUND;
 
 	/* Then fill the array */
-	list_for_each(lhandle, &efi_obj_list) {
-		struct efi_object *efiobj;
-		efiobj = list_entry(lhandle, struct efi_object, link);
-		if (!efi_search(search_type, protocol, search_key, efiobj)) {
-			*(buffer++) = efiobj->handle;
-		}
+	list_for_each_entry(efiobj, &efi_obj_list, link) {
+		if (!efi_search(search_type, protocol, search_key, efiobj))
+			*buffer++ = efiobj->handle;
 	}
 
 	return EFI_SUCCESS;
@@ -1009,7 +1083,7 @@ static efi_status_t efi_locate_handle(
 static efi_status_t EFIAPI efi_locate_handle_ext(
 			enum efi_locate_search_type search_type,
 			const efi_guid_t *protocol, void *search_key,
-			unsigned long *buffer_size, efi_handle_t *buffer)
+			efi_uintn_t *buffer_size, efi_handle_t *buffer)
 {
 	EFI_ENTRY("%d, %pUl, %p, %p, %p", search_type, protocol, search_key,
 		  buffer_size, buffer);
@@ -1018,36 +1092,6 @@ static efi_status_t EFIAPI efi_locate_handle_ext(
 			buffer_size, buffer));
 }
 
-/*
- * Get the device path and handle of an device implementing a protocol.
- *
- * This function implements the LocateDevicePath service.
- * See the Unified Extensible Firmware Interface (UEFI) specification
- * for details.
- *
- * @protocol		GUID of the protocol
- * @device_path		device path
- * @device		handle of the device
- * @return		status code
- */
-static efi_status_t EFIAPI efi_locate_device_path(
-			const efi_guid_t *protocol,
-			struct efi_device_path **device_path,
-			efi_handle_t *device)
-{
-	struct efi_object *efiobj;
-
-	EFI_ENTRY("%pUl, %p, %p", protocol, device_path, device);
-
-	efiobj = efi_dp_find_obj(*device_path, device_path);
-	if (!efiobj)
-		return EFI_EXIT(EFI_NOT_FOUND);
-
-	*device = efiobj->handle;
-
-	return EFI_EXIT(EFI_SUCCESS);
-}
-
 /* Collapses configuration table entries, removing index i */
 static void efi_remove_configuration_table(int i)
 {
@@ -1131,34 +1175,47 @@ void efi_setup_loaded_image(struct efi_loaded_image *info, struct efi_object *ob
 			    struct efi_device_path *device_path,
 			    struct efi_device_path *file_path)
 {
+	efi_status_t ret;
+
+	/* Add internal object to object list */
+	efi_add_handle(obj);
+	/* efi_exit() assumes that the handle points to the info */
 	obj->handle = info;
 
+	info->file_path = file_path;
+	if (device_path)
+		info->device_handle = efi_dp_find_obj(device_path, NULL);
+
 	/*
 	 * When asking for the device path interface, return
 	 * bootefi_device_path
 	 */
-	obj->protocols[0].guid = &efi_guid_device_path;
-	obj->protocols[0].protocol_interface = device_path;
+	ret = efi_add_protocol(obj->handle, &efi_guid_device_path, device_path);
+	if (ret != EFI_SUCCESS)
+		goto failure;
 
 	/*
 	 * When asking for the loaded_image interface, just
 	 * return handle which points to loaded_image_info
 	 */
-	obj->protocols[1].guid = &efi_guid_loaded_image;
-	obj->protocols[1].protocol_interface = info;
-
-	obj->protocols[2].guid = &efi_guid_console_control;
-	obj->protocols[2].protocol_interface = (void *)&efi_console_control;
+	ret = efi_add_protocol(obj->handle, &efi_guid_loaded_image, info);
+	if (ret != EFI_SUCCESS)
+		goto failure;
 
-	obj->protocols[3].guid = &efi_guid_device_path_to_text_protocol;
-	obj->protocols[3].protocol_interface =
-		(void *)&efi_device_path_to_text;
+	ret = efi_add_protocol(obj->handle, &efi_guid_console_control,
+			       (void *)&efi_console_control);
+	if (ret != EFI_SUCCESS)
+		goto failure;
 
-	info->file_path = file_path;
-	if (device_path)
-		info->device_handle = efi_dp_find_obj(device_path, NULL);
+	ret = efi_add_protocol(obj->handle,
+			       &efi_guid_device_path_to_text_protocol,
+			       (void *)&efi_device_path_to_text);
+	if (ret != EFI_SUCCESS)
+		goto failure;
 
-	list_add_tail(&obj->link, &efi_obj_list);
+	return;
+failure:
+	printf("ERROR: Failure to install protocols for loaded image\n");
 }
 
 /*
@@ -1273,7 +1330,9 @@ static efi_status_t EFIAPI efi_load_image(bool boot_policy,
 		return EFI_EXIT(EFI_UNSUPPORTED);
 	}
 
-	*image_handle = info;
+	info->system_table = &systab;
+	info->parent_handle = parent_image;
+	*image_handle = obj->handle;
 
 	return EFI_EXIT(EFI_SUCCESS);
 }
@@ -1335,6 +1394,17 @@ static efi_status_t EFIAPI efi_exit(efi_handle_t image_handle,
 			efi_status_t exit_status, unsigned long exit_data_size,
 			int16_t *exit_data)
 {
+	/*
+	 * We require that the handle points to the original loaded
+	 * image protocol interface.
+	 *
+	 * For getting the longjmp address this is safer than locating
+	 * the protocol because the protocol may have been reinstalled
+	 * pointing to another memory location.
+	 *
+	 * TODO: We should call the unload procedure of the loaded
+	 *	 image protocol.
+	 */
 	struct efi_loaded_image *loaded_image_info = (void*)image_handle;
 
 	EFI_ENTRY("%p, %ld, %ld, %p", image_handle, exit_status,
@@ -1356,26 +1426,6 @@ static efi_status_t EFIAPI efi_exit(efi_handle_t image_handle,
 }
 
 /*
- * Find the internal EFI object for a handle.
- *
- * @handle	handle to find
- * @return	EFI object
- */
-static struct efi_object *efi_search_obj(void *handle)
-{
-	struct list_head *lhandle;
-
-	list_for_each(lhandle, &efi_obj_list) {
-		struct efi_object *efiobj;
-		efiobj = list_entry(lhandle, struct efi_object, link);
-		if (efiobj->handle == handle)
-			return efiobj;
-	}
-
-	return NULL;
-}
-
-/*
  * Unload an EFI image.
  *
  * This function implements the UnloadImage service.
@@ -1450,6 +1500,7 @@ static efi_status_t EFIAPI efi_exit_boot_services(void *image_handle,
 	bootm_disable_interrupts();
 
 	/* Give the payload some time to boot */
+	efi_set_watchdog(0);
 	WATCHDOG_RESET();
 
 	return EFI_EXIT(EFI_SUCCESS);
@@ -1493,7 +1544,7 @@ static efi_status_t EFIAPI efi_stall(unsigned long microseconds)
 /*
  * Reset the watchdog timer.
  *
- * This function implements the WatchdogTimer service.
+ * This function implements the SetWatchdogTimer service.
  * See the Unified Extensible Firmware Interface (UEFI) specification
  * for details.
  *
@@ -1510,7 +1561,7 @@ static efi_status_t EFIAPI efi_set_watchdog_timer(unsigned long timeout,
 {
 	EFI_ENTRY("%ld, 0x%"PRIx64", %ld, %p", timeout, watchdog_code,
 		  data_size, watchdog_data);
-	return efi_unsupported(__func__);
+	return EFI_EXIT(efi_set_watchdog(timeout));
 }
 
 /*
@@ -1597,7 +1648,7 @@ static efi_status_t EFIAPI efi_close_protocol(void *handle,
 static efi_status_t EFIAPI efi_open_protocol_information(efi_handle_t handle,
 			const efi_guid_t *protocol,
 			struct efi_open_protocol_info_entry **entry_buffer,
-			unsigned long *entry_count)
+			efi_uintn_t *entry_count)
 {
 	EFI_ENTRY("%p, %pUl, %p, %p", handle, protocol, entry_buffer,
 		  entry_count);
@@ -1618,12 +1669,11 @@ static efi_status_t EFIAPI efi_open_protocol_information(efi_handle_t handle,
  */
 static efi_status_t EFIAPI efi_protocols_per_handle(void *handle,
 			efi_guid_t ***protocol_buffer,
-			unsigned long *protocol_buffer_count)
+			efi_uintn_t *protocol_buffer_count)
 {
 	unsigned long buffer_size;
 	struct efi_object *efiobj;
-	unsigned long i, j;
-	struct list_head *lhandle;
+	struct list_head *protocol_handle;
 	efi_status_t r;
 
 	EFI_ENTRY("%p, %p, %p", handle, protocol_buffer,
@@ -1634,36 +1684,33 @@ static efi_status_t EFIAPI efi_protocols_per_handle(void *handle,
 
 	*protocol_buffer = NULL;
 	*protocol_buffer_count = 0;
-	list_for_each(lhandle, &efi_obj_list) {
-		efiobj = list_entry(lhandle, struct efi_object, link);
 
-		if (efiobj->handle != handle)
-			continue;
+	efiobj = efi_search_obj(handle);
+	if (!efiobj)
+		return EFI_EXIT(EFI_INVALID_PARAMETER);
 
-		/* Count protocols */
-		for (i = 0; i < ARRAY_SIZE(efiobj->protocols); i++) {
-			if (efiobj->protocols[i].guid)
-				++*protocol_buffer_count;
-		}
-		/* Copy guids */
-		if (*protocol_buffer_count) {
-			buffer_size = sizeof(efi_guid_t *) *
-					*protocol_buffer_count;
-			r = efi_allocate_pool(EFI_ALLOCATE_ANY_PAGES,
-					      buffer_size,
-					      (void **)protocol_buffer);
-			if (r != EFI_SUCCESS)
-				return EFI_EXIT(r);
-			j = 0;
-			for (i = 0; i < ARRAY_SIZE(efiobj->protocols); ++i) {
-				if (efiobj->protocols[i].guid) {
-					(*protocol_buffer)[j] = (void *)
-						efiobj->protocols[i].guid;
-					++j;
-				}
-			}
+	/* Count protocols */
+	list_for_each(protocol_handle, &efiobj->protocols) {
+		++*protocol_buffer_count;
+	}
+
+	/* Copy guids */
+	if (*protocol_buffer_count) {
+		size_t j = 0;
+
+		buffer_size = sizeof(efi_guid_t *) * *protocol_buffer_count;
+		r = efi_allocate_pool(EFI_ALLOCATE_ANY_PAGES, buffer_size,
+				      (void **)protocol_buffer);
+		if (r != EFI_SUCCESS)
+			return EFI_EXIT(r);
+		list_for_each(protocol_handle, &efiobj->protocols) {
+			struct efi_handler *protocol;
+
+			protocol = list_entry(protocol_handle,
+					      struct efi_handler, link);
+			(*protocol_buffer)[j] = (void *)protocol->guid;
+			++j;
 		}
-		break;
 	}
 
 	return EFI_EXIT(EFI_SUCCESS);
@@ -1686,10 +1733,10 @@ static efi_status_t EFIAPI efi_protocols_per_handle(void *handle,
 static efi_status_t EFIAPI efi_locate_handle_buffer(
 			enum efi_locate_search_type search_type,
 			const efi_guid_t *protocol, void *search_key,
-			unsigned long *no_handles, efi_handle_t **buffer)
+			efi_uintn_t *no_handles, efi_handle_t **buffer)
 {
 	efi_status_t r;
-	unsigned long buffer_size = 0;
+	efi_uintn_t buffer_size = 0;
 
 	EFI_ENTRY("%d, %pUl, %p, %p, %p", search_type, protocol, search_key,
 		  no_handles, buffer);
@@ -1733,29 +1780,23 @@ static efi_status_t EFIAPI efi_locate_protocol(const efi_guid_t *protocol,
 					       void **protocol_interface)
 {
 	struct list_head *lhandle;
-	int i;
+	efi_status_t ret;
 
 	EFI_ENTRY("%pUl, %p, %p", protocol, registration, protocol_interface);
 
 	if (!protocol || !protocol_interface)
 		return EFI_EXIT(EFI_INVALID_PARAMETER);
 
-	EFI_PRINT_GUID("protocol", protocol);
-
 	list_for_each(lhandle, &efi_obj_list) {
 		struct efi_object *efiobj;
+		struct efi_handler *handler;
 
 		efiobj = list_entry(lhandle, struct efi_object, link);
-		for (i = 0; i < ARRAY_SIZE(efiobj->protocols); i++) {
-			struct efi_handler *handler = &efiobj->protocols[i];
-
-			if (!handler->guid)
-				continue;
-			if (!guidcmp(handler->guid, protocol)) {
-				*protocol_interface =
-					handler->protocol_interface;
-				return EFI_EXIT(EFI_SUCCESS);
-			}
+
+		ret = efi_search_protocol(efiobj->handle, protocol, &handler);
+		if (ret == EFI_SUCCESS) {
+			*protocol_interface = handler->protocol_interface;
+			return EFI_EXIT(EFI_SUCCESS);
 		}
 	}
 	*protocol_interface = NULL;
@@ -1764,6 +1805,82 @@ static efi_status_t EFIAPI efi_locate_protocol(const efi_guid_t *protocol,
 }
 
 /*
+ * Get the device path and handle of an device implementing a protocol.
+ *
+ * This function implements the LocateDevicePath service.
+ * See the Unified Extensible Firmware Interface (UEFI) specification
+ * for details.
+ *
+ * @protocol		GUID of the protocol
+ * @device_path		device path
+ * @device		handle of the device
+ * @return		status code
+ */
+static efi_status_t EFIAPI efi_locate_device_path(
+			const efi_guid_t *protocol,
+			struct efi_device_path **device_path,
+			efi_handle_t *device)
+{
+	struct efi_device_path *dp;
+	size_t i;
+	struct efi_handler *handler;
+	efi_handle_t *handles;
+	size_t len, len_dp;
+	size_t len_best = 0;
+	efi_uintn_t no_handles;
+	u8 *remainder;
+	efi_status_t ret;
+
+	EFI_ENTRY("%pUl, %p, %p", protocol, device_path, device);
+
+	if (!protocol || !device_path || !*device_path || !device) {
+		ret = EFI_INVALID_PARAMETER;
+		goto out;
+	}
+
+	/* Find end of device path */
+	len = efi_dp_size(*device_path);
+
+	/* Get all handles implementing the protocol */
+	ret = EFI_CALL(efi_locate_handle_buffer(BY_PROTOCOL, protocol, NULL,
+						&no_handles, &handles));
+	if (ret != EFI_SUCCESS)
+		goto out;
+
+	for (i = 0; i < no_handles; ++i) {
+		/* Find the device path protocol */
+		ret = efi_search_protocol(handles[i], &efi_guid_device_path,
+					  &handler);
+		if (ret != EFI_SUCCESS)
+			continue;
+		dp = (struct efi_device_path *)handler->protocol_interface;
+		len_dp = efi_dp_size(dp);
+		/*
+		 * This handle can only be a better fit
+		 * if its device path length is longer than the best fit and
+		 * if its device path length is shorter of equal the searched
+		 * device path.
+		 */
+		if (len_dp <= len_best || len_dp > len)
+			continue;
+		/* Check if dp is a subpath of device_path */
+		if (memcmp(*device_path, dp, len_dp))
+			continue;
+		*device = handles[i];
+		len_best = len_dp;
+	}
+	if (len_best) {
+		remainder = (u8 *)*device_path + len_best;
+		*device_path = (struct efi_device_path *)remainder;
+		ret = EFI_SUCCESS;
+	} else {
+		ret = EFI_NOT_FOUND;
+	}
+out:
+	return EFI_EXIT(ret);
+}
+
+/*
  * Install multiple protocol interfaces.
  *
  * This function implements the MultipleProtocolInterfaces service.
@@ -1795,9 +1912,10 @@ static efi_status_t EFIAPI efi_install_multiple_protocol_interfaces(
 		if (!protocol)
 			break;
 		protocol_interface = va_arg(argptr, void*);
-		r = efi_install_protocol_interface(handle, protocol,
-						   EFI_NATIVE_INTERFACE,
-						   protocol_interface);
+		r = EFI_CALL(efi_install_protocol_interface(
+						handle, protocol,
+						EFI_NATIVE_INTERFACE,
+						protocol_interface));
 		if (r != EFI_SUCCESS)
 			break;
 		i++;
@@ -1806,13 +1924,13 @@ static efi_status_t EFIAPI efi_install_multiple_protocol_interfaces(
 	if (r == EFI_SUCCESS)
 		return EFI_EXIT(r);
 
-	/* If an error occured undo all changes. */
+	/* If an error occurred undo all changes. */
 	va_start(argptr, handle);
 	for (; i; --i) {
 		protocol = va_arg(argptr, efi_guid_t*);
 		protocol_interface = va_arg(argptr, void*);
-		efi_uninstall_protocol_interface(handle, protocol,
-						 protocol_interface);
+		EFI_CALL(efi_uninstall_protocol_interface(handle, protocol,
+							  protocol_interface));
 	}
 	va_end(argptr);
 
@@ -1835,7 +1953,45 @@ static efi_status_t EFIAPI efi_uninstall_multiple_protocol_interfaces(
 			void *handle, ...)
 {
 	EFI_ENTRY("%p", handle);
-	return EFI_EXIT(EFI_INVALID_PARAMETER);
+
+	va_list argptr;
+	const efi_guid_t *protocol;
+	void *protocol_interface;
+	efi_status_t r = EFI_SUCCESS;
+	size_t i = 0;
+
+	if (!handle)
+		return EFI_EXIT(EFI_INVALID_PARAMETER);
+
+	va_start(argptr, handle);
+	for (;;) {
+		protocol = va_arg(argptr, efi_guid_t*);
+		if (!protocol)
+			break;
+		protocol_interface = va_arg(argptr, void*);
+		r = EFI_CALL(efi_uninstall_protocol_interface(
+						handle, protocol,
+						protocol_interface));
+		if (r != EFI_SUCCESS)
+			break;
+		i++;
+	}
+	va_end(argptr);
+	if (r == EFI_SUCCESS)
+		return EFI_EXIT(r);
+
+	/* If an error occurred undo all changes. */
+	va_start(argptr, handle);
+	for (; i; --i) {
+		protocol = va_arg(argptr, efi_guid_t*);
+		protocol_interface = va_arg(argptr, void*);
+		EFI_CALL(efi_install_protocol_interface(&handle, protocol,
+							EFI_NATIVE_INTERFACE,
+							protocol_interface));
+	}
+	va_end(argptr);
+
+	return EFI_EXIT(r);
 }
 
 /*
@@ -1916,8 +2072,7 @@ static efi_status_t EFIAPI efi_open_protocol(
 			void **protocol_interface, void *agent_handle,
 			void *controller_handle, uint32_t attributes)
 {
-	struct list_head *lhandle;
-	int i;
+	struct efi_handler *handler;
 	efi_status_t r = EFI_INVALID_PARAMETER;
 
 	EFI_ENTRY("%p, %pUl, %p, %p, %p, 0x%x", handle, protocol,
@@ -1930,8 +2085,6 @@ static efi_status_t EFIAPI efi_open_protocol(
 		goto out;
 	}
 
-	EFI_PRINT_GUID("protocol", protocol);
-
 	switch (attributes) {
 	case EFI_OPEN_PROTOCOL_BY_HANDLE_PROTOCOL:
 	case EFI_OPEN_PROTOCOL_GET_PROTOCOL:
@@ -1952,33 +2105,12 @@ static efi_status_t EFIAPI efi_open_protocol(
 		goto out;
 	}
 
-	list_for_each(lhandle, &efi_obj_list) {
-		struct efi_object *efiobj;
-		efiobj = list_entry(lhandle, struct efi_object, link);
-
-		if (efiobj->handle != handle)
-			continue;
-
-		for (i = 0; i < ARRAY_SIZE(efiobj->protocols); i++) {
-			struct efi_handler *handler = &efiobj->protocols[i];
-			const efi_guid_t *hprotocol = handler->guid;
-			if (!hprotocol)
-				continue;
-			if (!guidcmp(hprotocol, protocol)) {
-				if (attributes !=
-				    EFI_OPEN_PROTOCOL_TEST_PROTOCOL) {
-					*protocol_interface =
-						handler->protocol_interface;
-				}
-				r = EFI_SUCCESS;
-				goto out;
-			}
-		}
-		goto unsupported;
-	}
+	r = efi_search_protocol(handle, protocol, &handler);
+	if (r != EFI_SUCCESS)
+		goto out;
 
-unsupported:
-	r = EFI_UNSUPPORTED;
+	if (attributes != EFI_OPEN_PROTOCOL_TEST_PROTOCOL)
+		*protocol_interface = handler->protocol_interface;
 out:
 	return EFI_EXIT(r);
 }
@@ -2020,9 +2152,9 @@ static const struct efi_boot_services efi_boot_services = {
 	.signal_event = efi_signal_event_ext,
 	.close_event = efi_close_event,
 	.check_event = efi_check_event,
-	.install_protocol_interface = efi_install_protocol_interface_ext,
+	.install_protocol_interface = efi_install_protocol_interface,
 	.reinstall_protocol_interface = efi_reinstall_protocol_interface,
-	.uninstall_protocol_interface = efi_uninstall_protocol_interface_ext,
+	.uninstall_protocol_interface = efi_uninstall_protocol_interface,
 	.handle_protocol = efi_handle_protocol,
 	.reserved = NULL,
 	.register_protocol_notify = efi_register_protocol_notify,
diff --git a/lib/efi_loader/efi_console.c b/lib/efi_loader/efi_console.c
index 01732aa..98497db 100644
--- a/lib/efi_loader/efi_console.c
+++ b/lib/efi_loader/efi_console.c
@@ -46,6 +46,10 @@ static struct cout_mode efi_cout_modes[] = {
 };
 
 const efi_guid_t efi_guid_console_control = CONSOLE_CONTROL_GUID;
+const efi_guid_t efi_guid_text_output_protocol =
+			EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL_GUID;
+const efi_guid_t efi_guid_text_input_protocol =
+			EFI_SIMPLE_TEXT_INPUT_PROTOCOL_GUID;
 
 #define cESC '\x1b'
 #define ESC "\x1b"
@@ -81,7 +85,7 @@ static efi_status_t EFIAPI efi_cin_lock_std_in(
 	return EFI_EXIT(EFI_UNSUPPORTED);
 }
 
-const struct efi_console_control_protocol efi_console_control = {
+struct efi_console_control_protocol efi_console_control = {
 	.get_mode = efi_cin_get_mode,
 	.set_mode = efi_cin_set_mode,
 	.lock_std_in = efi_cin_lock_std_in,
@@ -374,7 +378,7 @@ static efi_status_t EFIAPI efi_cout_enable_cursor(
 	return EFI_EXIT(EFI_SUCCESS);
 }
 
-const struct efi_simple_text_output_protocol efi_con_out = {
+struct efi_simple_text_output_protocol efi_con_out = {
 	.reset = efi_cout_reset,
 	.output_string = efi_cout_output_string,
 	.test_string = efi_cout_test_string,
@@ -490,23 +494,38 @@ static void EFIAPI efi_console_timer_notify(struct efi_event *event,
 }
 
 
-static struct efi_object efi_console_control_obj =
-	EFI_PROTOCOL_OBJECT(efi_guid_console_control, &efi_console_control);
-static struct efi_object efi_console_output_obj =
-	EFI_PROTOCOL_OBJECT(EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL_GUID, &efi_con_out);
-static struct efi_object efi_console_input_obj =
-	EFI_PROTOCOL_OBJECT(EFI_SIMPLE_TEXT_INPUT_PROTOCOL_GUID, &efi_con_in);
-
 /* This gets called from do_bootefi_exec(). */
 int efi_console_register(void)
 {
 	efi_status_t r;
+	struct efi_object *efi_console_control_obj;
+	struct efi_object *efi_console_output_obj;
+	struct efi_object *efi_console_input_obj;
 
-	/* Hook up to the device list */
-	list_add_tail(&efi_console_control_obj.link, &efi_obj_list);
-	list_add_tail(&efi_console_output_obj.link, &efi_obj_list);
-	list_add_tail(&efi_console_input_obj.link, &efi_obj_list);
+	/* Create handles */
+	r = efi_create_handle((void **)&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);
+	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);
+	if (r != EFI_SUCCESS)
+		goto out_of_memory;
+	r = efi_add_protocol(efi_console_input_obj->handle,
+			     &efi_guid_text_input_protocol, &efi_con_in);
+	if (r != EFI_SUCCESS)
+		goto out_of_memory;
 
+	/* Create console events */
 	r = efi_create_event(EVT_NOTIFY_WAIT, TPL_CALLBACK,
 			     efi_key_notify, NULL, &efi_con_in.wait_for_key);
 	if (r != EFI_SUCCESS) {
@@ -525,4 +544,7 @@ int efi_console_register(void)
 	if (r != EFI_SUCCESS)
 		printf("ERROR: Failed to set console timer\n");
 	return r;
+out_of_memory:
+	printf("ERROR: Out of meemory\n");
+	return r;
 }
diff --git a/lib/efi_loader/efi_device_path.c b/lib/efi_loader/efi_device_path.c
index f6e368e..b4e2f93 100644
--- a/lib/efi_loader/efi_device_path.c
+++ b/lib/efi_loader/efi_device_path.c
@@ -68,7 +68,8 @@ struct efi_device_path *efi_dp_next(const struct efi_device_path *dp)
  * representing a device with one representing a file on the device, or
  * a device with a parent device.
  */
-int efi_dp_match(struct efi_device_path *a, struct efi_device_path *b)
+int efi_dp_match(const struct efi_device_path *a,
+		 const struct efi_device_path *b)
 {
 	while (1) {
 		int ret;
@@ -127,32 +128,27 @@ static struct efi_object *find_obj(struct efi_device_path *dp, bool short_path,
 	struct efi_object *efiobj;
 
 	list_for_each_entry(efiobj, &efi_obj_list, link) {
-		int i;
-
-		for (i = 0; i < ARRAY_SIZE(efiobj->protocols); i++) {
-			struct efi_handler *handler = &efiobj->protocols[i];
-			struct efi_device_path *obj_dp;
-
-			if (!handler->guid)
-				break;
-
-			if (guidcmp(handler->guid, &efi_guid_device_path))
-				continue;
-
-			obj_dp = handler->protocol_interface;
-
-			do {
-				if (efi_dp_match(dp, obj_dp) == 0) {
-					if (rem) {
-						*rem = ((void *)dp) +
-							efi_dp_size(obj_dp);
-					}
-					return efiobj;
+		struct efi_handler *handler;
+		struct efi_device_path *obj_dp;
+		efi_status_t ret;
+
+		ret = efi_search_protocol(efiobj->handle,
+					  &efi_guid_device_path, &handler);
+		if (ret != EFI_SUCCESS)
+			continue;
+		obj_dp = handler->protocol_interface;
+
+		do {
+			if (efi_dp_match(dp, obj_dp) == 0) {
+				if (rem) {
+					*rem = ((void *)dp) +
+						efi_dp_size(obj_dp);
 				}
+				return efiobj;
+			}
 
-				obj_dp = shorten_path(efi_dp_next(obj_dp));
-			} while (short_path && obj_dp);
-		}
+			obj_dp = shorten_path(efi_dp_next(obj_dp));
+		} while (short_path && obj_dp);
 	}
 
 	return NULL;
@@ -427,10 +423,27 @@ static void *dp_part_fill(void *buf, struct blk_desc *desc, int part)
 			hddp->partmap_type = 2;
 		else
 			hddp->partmap_type = 1;
-		hddp->signature_type = desc->sig_type;
-		if (hddp->signature_type != 0)
+
+		switch (desc->sig_type) {
+		case SIG_TYPE_NONE:
+		default:
+			hddp->signature_type = 0;
+			memset(hddp->partition_signature, 0,
+			       sizeof(hddp->partition_signature));
+			break;
+		case SIG_TYPE_MBR:
+			hddp->signature_type = 1;
+			memset(hddp->partition_signature, 0,
+			       sizeof(hddp->partition_signature));
+			memcpy(hddp->partition_signature, &desc->mbr_sig,
+			       sizeof(desc->mbr_sig));
+			break;
+		case SIG_TYPE_GUID:
+			hddp->signature_type = 2;
 			memcpy(hddp->partition_signature, &desc->guid_sig,
 			       sizeof(hddp->partition_signature));
+			break;
+		}
 
 		buf = &hddp[1];
 	}
diff --git a/lib/efi_loader/efi_device_path_to_text.c b/lib/efi_loader/efi_device_path_to_text.c
index 6277133..7159c97 100644
--- a/lib/efi_loader/efi_device_path_to_text.c
+++ b/lib/efi_loader/efi_device_path_to_text.c
@@ -12,12 +12,31 @@
 #define MAC_OUTPUT_LEN 22
 #define UNKNOWN_OUTPUT_LEN 23
 
+#define MAX_NODE_LEN 512
+#define MAX_PATH_LEN 1024
+
 const efi_guid_t efi_guid_device_path_to_text_protocol =
 		EFI_DEVICE_PATH_TO_TEXT_PROTOCOL_GUID;
 
+static u16 *efi_str_to_u16(char *str)
+{
+	efi_uintn_t len;
+	u16 *out;
+	efi_status_t ret;
+
+	len = strlen(str) + 1;
+	ret = efi_allocate_pool(EFI_ALLOCATE_ANY_PAGES, len * sizeof(u16),
+				(void **)&out);
+	if (ret != EFI_SUCCESS)
+		return NULL;
+	ascii2unicode(out, str);
+	out[len - 1] = 0;
+	return out;
+}
+
 static char *dp_unknown(char *s, struct efi_device_path *dp)
 {
-	s += sprintf(s, "/UNKNOWN(%04x,%04x)", dp->type, dp->sub_type);
+	s += sprintf(s, "UNKNOWN(%04x,%04x)", dp->type, dp->sub_type);
 	return s;
 }
 
@@ -27,7 +46,7 @@ static char *dp_hardware(char *s, struct efi_device_path *dp)
 	case DEVICE_PATH_SUB_TYPE_MEMORY: {
 		struct efi_device_path_memory *mdp =
 			(struct efi_device_path_memory *)dp;
-		s += sprintf(s, "/MemoryMapped(0x%x,0x%llx,0x%llx)",
+		s += sprintf(s, "MemoryMapped(0x%x,0x%llx,0x%llx)",
 			     mdp->memory_type,
 			     mdp->start_address,
 			     mdp->end_address);
@@ -36,7 +55,7 @@ static char *dp_hardware(char *s, struct efi_device_path *dp)
 	case DEVICE_PATH_SUB_TYPE_VENDOR: {
 		struct efi_device_path_vendor *vdp =
 			(struct efi_device_path_vendor *)dp;
-		s += sprintf(s, "/VenHw(%pUl)", &vdp->guid);
+		s += sprintf(s, "VenHw(%pUl)", &vdp->guid);
 		break;
 	}
 	default:
@@ -52,7 +71,7 @@ static char *dp_acpi(char *s, struct efi_device_path *dp)
 	case DEVICE_PATH_SUB_TYPE_ACPI_DEVICE: {
 		struct efi_device_path_acpi_path *adp =
 			(struct efi_device_path_acpi_path *)dp;
-		s += sprintf(s, "/Acpi(PNP%04x", EISA_PNP_NUM(adp->hid));
+		s += sprintf(s, "Acpi(PNP%04x", EISA_PNP_NUM(adp->hid));
 		if (adp->uid)
 			s += sprintf(s, ",%d", adp->uid);
 		s += sprintf(s, ")");
@@ -71,7 +90,7 @@ static char *dp_msging(char *s, struct efi_device_path *dp)
 	case DEVICE_PATH_SUB_TYPE_MSG_USB: {
 		struct efi_device_path_usb *udp =
 			(struct efi_device_path_usb *)dp;
-		s += sprintf(s, "/Usb(0x%x,0x%x)", udp->parent_port_number,
+		s += sprintf(s, "Usb(0x%x,0x%x)", udp->parent_port_number,
 			     udp->usb_interface);
 		break;
 	}
@@ -82,7 +101,7 @@ static char *dp_msging(char *s, struct efi_device_path *dp)
 		if (mdp->if_type != 0 && mdp->if_type != 1)
 			break;
 
-		s += sprintf(s, "/MAC(%02x%02x%02x%02x%02x%02x,0x%1x)",
+		s += sprintf(s, "MAC(%02x%02x%02x%02x%02x%02x,0x%1x)",
 			mdp->mac.addr[0], mdp->mac.addr[1],
 			mdp->mac.addr[2], mdp->mac.addr[3],
 			mdp->mac.addr[4], mdp->mac.addr[5],
@@ -94,7 +113,7 @@ static char *dp_msging(char *s, struct efi_device_path *dp)
 		struct efi_device_path_usb_class *ucdp =
 			(struct efi_device_path_usb_class *)dp;
 
-		s += sprintf(s, "/USBClass(%x,%x,%x,%x,%x)",
+		s += sprintf(s, "USBClass(%x,%x,%x,%x,%x)",
 			ucdp->vendor_id, ucdp->product_id,
 			ucdp->device_class, ucdp->device_subclass,
 			ucdp->device_protocol);
@@ -108,7 +127,7 @@ static char *dp_msging(char *s, struct efi_device_path *dp)
 					"SDCard" : "MMC";
 		struct efi_device_path_sd_mmc_path *sddp =
 			(struct efi_device_path_sd_mmc_path *)dp;
-		s += sprintf(s, "/%s(Slot%u)", typename, sddp->slot_number);
+		s += sprintf(s, "%s(Slot%u)", typename, sddp->slot_number);
 		break;
 	}
 	default:
@@ -128,17 +147,19 @@ static char *dp_media(char *s, struct efi_device_path *dp)
 
 		switch (hddp->signature_type) {
 		case SIG_TYPE_MBR:
-			s += sprintf(s, "/HD(Part%d,Sig%08x)",
+			s += sprintf(s, "HD(Part%d,Sig%08x)",
 				     hddp->partition_number,
 				     *(uint32_t *)sig);
 			break;
 		case SIG_TYPE_GUID:
-			s += sprintf(s, "/HD(Part%d,Sig%pUl)",
+			s += sprintf(s, "HD(Part%d,Sig%pUl)",
 				     hddp->partition_number, sig);
+			break;
 		default:
-			s += sprintf(s, "/HD(Part%d,MBRType=%02x,SigType=%02x)",
+			s += sprintf(s, "HD(Part%d,MBRType=%02x,SigType=%02x)",
 				     hddp->partition_number, hddp->partmap_type,
 				     hddp->signature_type);
+			break;
 		}
 
 		break;
@@ -146,14 +167,16 @@ static char *dp_media(char *s, struct efi_device_path *dp)
 	case DEVICE_PATH_SUB_TYPE_CDROM_PATH: {
 		struct efi_device_path_cdrom_path *cddp =
 			(struct efi_device_path_cdrom_path *)dp;
-		s += sprintf(s, "/CDROM(0x%x)", cddp->boot_entry);
+		s += sprintf(s, "CDROM(0x%x)", cddp->boot_entry);
 		break;
 	}
 	case DEVICE_PATH_SUB_TYPE_FILE_PATH: {
 		struct efi_device_path_file_path *fp =
 			(struct efi_device_path_file_path *)dp;
 		int slen = (dp->length - sizeof(*dp)) / 2;
-		s += sprintf(s, "/%-*ls", slen, fp->str);
+		if (slen > MAX_NODE_LEN - 2)
+			slen = MAX_NODE_LEN - 2;
+		s += sprintf(s, "%-.*ls", slen, fp->str);
 		break;
 	}
 	default:
@@ -163,95 +186,119 @@ static char *dp_media(char *s, struct efi_device_path *dp)
 	return s;
 }
 
-static uint16_t *efi_convert_device_node_to_text(
-		struct efi_device_path *dp,
-		bool display_only,
-		bool allow_shortcuts)
+/*
+ * Converts a single node to a char string.
+ *
+ * @buffer		output buffer
+ * @dp			device path or node
+ * @return		end of string
+ */
+static char *efi_convert_single_device_node_to_text(
+		char *buffer,
+		struct efi_device_path *dp)
 {
-	unsigned long len;
-	efi_status_t r;
-	char buf[512];  /* this ought be be big enough for worst case */
-	char *str = buf;
-	uint16_t *out;
-
-	while (dp) {
-		switch (dp->type) {
-		case DEVICE_PATH_TYPE_HARDWARE_DEVICE:
-			str = dp_hardware(str, dp);
-			break;
-		case DEVICE_PATH_TYPE_ACPI_DEVICE:
-			str = dp_acpi(str, dp);
-			break;
-		case DEVICE_PATH_TYPE_MESSAGING_DEVICE:
-			str = dp_msging(str, dp);
-			break;
-		case DEVICE_PATH_TYPE_MEDIA_DEVICE:
-			str = dp_media(str, dp);
-			break;
-		default:
-			str = dp_unknown(str, dp);
-		}
+	char *str = buffer;
 
-		dp = efi_dp_next(dp);
+	switch (dp->type) {
+	case DEVICE_PATH_TYPE_HARDWARE_DEVICE:
+		str = dp_hardware(str, dp);
+		break;
+	case DEVICE_PATH_TYPE_ACPI_DEVICE:
+		str = dp_acpi(str, dp);
+		break;
+	case DEVICE_PATH_TYPE_MESSAGING_DEVICE:
+		str = dp_msging(str, dp);
+		break;
+	case DEVICE_PATH_TYPE_MEDIA_DEVICE:
+		str = dp_media(str, dp);
+		break;
+	default:
+		str = dp_unknown(str, dp);
 	}
 
-	*str++ = '\0';
-
-	len = str - buf;
-	r = efi_allocate_pool(EFI_ALLOCATE_ANY_PAGES, 2 * len, (void **)&out);
-	if (r != EFI_SUCCESS)
-		return NULL;
-
-	ascii2unicode(out, buf);
-	out[len - 1] = 0;
-
-	return out;
+	*str = '\0';
+	return str;
 }
 
-/* helper for debug prints.. efi_free_pool() the result. */
-uint16_t *efi_dp_str(struct efi_device_path *dp)
-{
-	return efi_convert_device_node_to_text(dp, true, true);
-}
-
-
-static uint16_t EFIAPI *efi_convert_device_node_to_text_ext(
+/*
+ * This function implements the ConvertDeviceNodeToText service of the
+ * EFI_DEVICE_PATH_TO_TEXT_PROTOCOL.
+ * See the Unified Extensible Firmware Interface (UEFI) specification
+ * for details.
+ *
+ * device_node		device node to be converted
+ * display_only		true if the shorter text represenation shall be used
+ * allow_shortcuts	true if shortcut forms may be used
+ * @return		text represenation of the device path
+ *			NULL if out of memory of device_path is NULL
+ */
+static uint16_t EFIAPI *efi_convert_device_node_to_text(
 		struct efi_device_path *device_node,
 		bool display_only,
 		bool allow_shortcuts)
 {
-	uint16_t *buffer;
+	char str[MAX_NODE_LEN];
+	uint16_t *text = NULL;
 
 	EFI_ENTRY("%p, %d, %d", device_node, display_only, allow_shortcuts);
 
-	buffer = efi_convert_device_node_to_text(device_node, display_only,
-						 allow_shortcuts);
+	if (!device_node)
+		goto out;
+	efi_convert_single_device_node_to_text(str, device_node);
+
+	text = efi_str_to_u16(str);
 
+out:
 	EFI_EXIT(EFI_SUCCESS);
-	return buffer;
+	return text;
 }
 
+/*
+ * This function implements the ConvertDevicePathToText service of the
+ * EFI_DEVICE_PATH_TO_TEXT_PROTOCOL.
+ * See the Unified Extensible Firmware Interface (UEFI) specification
+ * for details.
+ *
+ * device_path		device path to be converted
+ * display_only		true if the shorter text represenation shall be used
+ * allow_shortcuts	true if shortcut forms may be used
+ * @return		text represenation of the device path
+ *			NULL if out of memory of device_path is NULL
+ */
 static uint16_t EFIAPI *efi_convert_device_path_to_text(
 		struct efi_device_path *device_path,
 		bool display_only,
 		bool allow_shortcuts)
 {
-	uint16_t *buffer;
+	uint16_t *text = NULL;
+	char buffer[MAX_PATH_LEN];
+	char *str = buffer;
 
 	EFI_ENTRY("%p, %d, %d", device_path, display_only, allow_shortcuts);
 
-	/*
-	 * Our device paths are all of depth one. So its is sufficient to
-	 * to convert the first node.
-	 */
-	buffer = efi_convert_device_node_to_text(device_path, display_only,
-						 allow_shortcuts);
+	if (!device_path)
+		goto out;
+	while (device_path &&
+	       str + MAX_NODE_LEN < buffer + MAX_PATH_LEN) {
+		*str++ = '/';
+		str = efi_convert_single_device_node_to_text(str, device_path);
+		device_path = efi_dp_next(device_path);
+	}
+
+	text = efi_str_to_u16(buffer);
 
+out:
 	EFI_EXIT(EFI_SUCCESS);
-	return buffer;
+	return text;
+}
+
+/* helper for debug prints.. efi_free_pool() the result. */
+uint16_t *efi_dp_str(struct efi_device_path *dp)
+{
+	return EFI_CALL(efi_convert_device_path_to_text(dp, true, true));
 }
 
 const struct efi_device_path_to_text_protocol efi_device_path_to_text = {
-	.convert_device_node_to_text = efi_convert_device_node_to_text_ext,
+	.convert_device_node_to_text = efi_convert_device_node_to_text,
 	.convert_device_path_to_text = efi_convert_device_path_to_text,
 };
diff --git a/lib/efi_loader/efi_disk.c b/lib/efi_loader/efi_disk.c
index e61dbc8..4e457a8 100644
--- a/lib/efi_loader/efi_disk.c
+++ b/lib/efi_loader/efi_disk.c
@@ -196,6 +196,15 @@ efi_fs_from_path(struct efi_device_path *fp)
 	return diskobj->volume;
 }
 
+/*
+ * Create a device for a disk
+ *
+ * @name	not used
+ * @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
+ */
 static void efi_disk_add_dev(const char *name,
 			     const char *if_typename,
 			     struct blk_desc *desc,
@@ -204,29 +213,39 @@ static void efi_disk_add_dev(const char *name,
 			     unsigned int part)
 {
 	struct efi_disk_obj *diskobj;
+	efi_status_t ret;
 
 	/* Don't add empty devices */
 	if (!desc->lba)
 		return;
 
 	diskobj = calloc(1, sizeof(*diskobj));
+	if (!diskobj)
+		goto out_of_memory;
+
+	/* Hook up to the device list */
+	efi_add_handle(&diskobj->parent);
 
 	/* Fill in object data */
 	diskobj->dp = efi_dp_from_part(desc, part);
 	diskobj->part = part;
-	diskobj->parent.protocols[0].guid = &efi_block_io_guid;
-	diskobj->parent.protocols[0].protocol_interface = &diskobj->ops;
-	diskobj->parent.protocols[1].guid = &efi_guid_device_path;
-	diskobj->parent.protocols[1].protocol_interface = diskobj->dp;
+	ret = efi_add_protocol(diskobj->parent.handle, &efi_block_io_guid,
+			       &diskobj->ops);
+	if (ret != EFI_SUCCESS)
+		goto out_of_memory;
+	ret = efi_add_protocol(diskobj->parent.handle, &efi_guid_device_path,
+			       diskobj->dp);
+	if (ret != EFI_SUCCESS)
+		goto out_of_memory;
 	if (part >= 1) {
 		diskobj->volume = efi_simple_file_system(desc, part,
 							 diskobj->dp);
-		diskobj->parent.protocols[2].guid =
-			&efi_simple_file_system_protocol_guid;
-		diskobj->parent.protocols[2].protocol_interface =
-			diskobj->volume;
+		ret = efi_add_protocol(diskobj->parent.handle,
+				       &efi_simple_file_system_protocol_guid,
+				       &diskobj->volume);
+		if (ret != EFI_SUCCESS)
+			goto out_of_memory;
 	}
-	diskobj->parent.handle = diskobj;
 	diskobj->ops = block_io_disk_template;
 	diskobj->ifname = if_typename;
 	diskobj->dev_index = dev_index;
@@ -240,26 +259,22 @@ static void efi_disk_add_dev(const char *name,
 	diskobj->media.io_align = desc->blksz;
 	diskobj->media.last_block = desc->lba - offset;
 	diskobj->ops.media = &diskobj->media;
-
-	/* Hook up to the device list */
-	list_add_tail(&diskobj->parent.link, &efi_obj_list);
+	return;
+out_of_memory:
+	printf("ERROR: Out of memory\n");
 }
 
-static int efi_disk_create_eltorito(struct blk_desc *desc,
-				    const char *if_typename,
-				    int diskid,
-				    const char *pdevname)
+static int efi_disk_create_partitions(struct blk_desc *desc,
+				      const char *if_typename,
+				      int diskid,
+				      const char *pdevname)
 {
 	int disks = 0;
-#if CONFIG_IS_ENABLED(ISO_PARTITION)
 	char devname[32] = { 0 }; /* dp->str is u16[32] long */
 	disk_partition_t info;
 	int part;
 
-	if (desc->part_type != PART_TYPE_ISO)
-		return 0;
-
-	/* and devices for each partition: */
+	/* Add devices for each partition */
 	for (part = 1; part <= MAX_SEARCH_PARTITIONS; part++) {
 		if (part_get_info(desc, part, &info))
 			continue;
@@ -270,10 +285,6 @@ static int efi_disk_create_eltorito(struct blk_desc *desc,
 		disks++;
 	}
 
-	/* ... and add block device: */
-	efi_disk_add_dev(devname, if_typename, desc, diskid, 0, 0);
-#endif
-
 	return disks;
 }
 
@@ -299,31 +310,18 @@ int efi_disk_register(void)
 	     uclass_next_device_check(&dev)) {
 		struct blk_desc *desc = dev_get_uclass_platdata(dev);
 		const char *if_typename = dev->driver->name;
-		disk_partition_t info;
-		int part;
 
 		printf("Scanning disk %s...\n", dev->name);
 
-		/* add devices for each partition: */
-		for (part = 1; part <= MAX_SEARCH_PARTITIONS; part++) {
-			if (part_get_info(desc, part, &info))
-				continue;
-			efi_disk_add_dev(dev->name, if_typename, desc,
-					 desc->devnum, 0, part);
-		}
-
-		/* ... and add block device: */
+		/* Add block device for the full device */
 		efi_disk_add_dev(dev->name, if_typename, desc,
 				 desc->devnum, 0, 0);
 
 		disks++;
 
-		/*
-		* El Torito images show up as block devices in an EFI world,
-		* so let's create them here
-		*/
-		disks += efi_disk_create_eltorito(desc, if_typename,
-						  desc->devnum, dev->name);
+		/* Partitions show up as block devices in EFI */
+		disks += efi_disk_create_partitions(desc, if_typename,
+						    desc->devnum, dev->name);
 	}
 #else
 	int i, if_type;
@@ -342,8 +340,6 @@ int efi_disk_register(void)
 		for (i = 0; i < 4; i++) {
 			struct blk_desc *desc;
 			char devname[32] = { 0 }; /* dp->str is u16[32] long */
-			disk_partition_t info;
-			int part;
 
 			desc = blk_get_devnum_by_type(if_type, i);
 			if (!desc)
@@ -354,24 +350,13 @@ int efi_disk_register(void)
 			snprintf(devname, sizeof(devname), "%s%d",
 				 if_typename, i);
 
-			/* add devices for each partition: */
-			for (part = 1; part <= MAX_SEARCH_PARTITIONS; part++) {
-				if (part_get_info(desc, part, &info))
-					continue;
-				efi_disk_add_dev(devname, if_typename, desc,
-						 i, 0, part);
-			}
-
-			/* ... and add block device: */
+			/* Add block device for the full device */
 			efi_disk_add_dev(devname, if_typename, desc, i, 0, 0);
 			disks++;
 
-			/*
-			 * El Torito images show up as block devices
-			 * in an EFI world, so let's create them here
-			 */
-			disks += efi_disk_create_eltorito(desc, if_typename,
-							  i, devname);
+			/* Partitions show up as block devices in EFI */
+			disks += efi_disk_create_partitions(desc, if_typename,
+							    i, devname);
 		}
 	}
 #endif
diff --git a/lib/efi_loader/efi_gop.c b/lib/efi_loader/efi_gop.c
index 411a8c9..3caddd5 100644
--- a/lib/efi_loader/efi_gop.c
+++ b/lib/efi_loader/efi_gop.c
@@ -32,7 +32,7 @@ struct efi_gop_obj {
 };
 
 static efi_status_t EFIAPI gop_query_mode(struct efi_gop *this, u32 mode_number,
-					  unsigned long *size_of_info,
+					  efi_uintn_t *size_of_info,
 					  struct efi_gop_mode_info **info)
 {
 	struct efi_gop_obj *gopobj;
@@ -56,17 +56,17 @@ static efi_status_t EFIAPI gop_set_mode(struct efi_gop *this, u32 mode_number)
 	return EFI_EXIT(EFI_SUCCESS);
 }
 
-static efi_status_t EFIAPI gop_blt(struct efi_gop *this, void *buffer,
-				   unsigned long operation, unsigned long sx,
-				   unsigned long sy, unsigned long dx,
-				   unsigned long dy, unsigned long width,
-				   unsigned long height, unsigned long delta)
+efi_status_t EFIAPI gop_blt(struct efi_gop *this, void *buffer,
+			    u32 operation, efi_uintn_t sx,
+			    efi_uintn_t sy, efi_uintn_t dx,
+			    efi_uintn_t dy, efi_uintn_t width,
+			    efi_uintn_t height, efi_uintn_t delta)
 {
 	struct efi_gop_obj *gopobj = container_of(this, struct efi_gop_obj, ops);
 	int i, j, line_len16, line_len32;
 	void *fb;
 
-	EFI_ENTRY("%p, %p, %lx, %lx, %lx, %lx, %lx, %lx, %lx, %lx", this,
+	EFI_ENTRY("%p, %p, %u, %zu, %zu, %zu, %zu, %zu, %zu, %zu", this,
 		  buffer, operation, sx, sy, dx, dy, width, height, delta);
 
 	if (operation != EFI_BLT_BUFFER_TO_VIDEO)
@@ -132,6 +132,7 @@ int efi_gop_register(void)
 	u32 bpix, col, row;
 	u64 fb_base, fb_size;
 	void *fb;
+	efi_status_t ret;
 
 #ifdef CONFIG_DM_VIDEO
 	struct udevice *vdev;
@@ -173,11 +174,21 @@ int efi_gop_register(void)
 	}
 
 	gopobj = calloc(1, sizeof(*gopobj));
+	if (!gopobj) {
+		printf("ERROR: Out of memory\n");
+		return 1;
+	}
+
+	/* Hook up to the device list */
+	efi_add_handle(&gopobj->parent);
 
 	/* Fill in object data */
-	gopobj->parent.protocols[0].guid = &efi_gop_guid;
-	gopobj->parent.protocols[0].protocol_interface = &gopobj->ops;
-	gopobj->parent.handle = &gopobj->ops;
+	ret = efi_add_protocol(gopobj->parent.handle, &efi_gop_guid,
+			       &gopobj->ops);
+	if (ret != EFI_SUCCESS) {
+		printf("ERROR: Out of memory\n");
+		return 1;
+	}
 	gopobj->ops.query_mode = gop_query_mode;
 	gopobj->ops.set_mode = gop_set_mode;
 	gopobj->ops.blt = gop_blt;
@@ -206,8 +217,5 @@ int efi_gop_register(void)
 	gopobj->bpix = bpix;
 	gopobj->fb = fb;
 
-	/* Hook up to the device list */
-	list_add_tail(&gopobj->parent.link, &efi_obj_list);
-
 	return 0;
 }
diff --git a/lib/efi_loader/efi_memory.c b/lib/efi_loader/efi_memory.c
index d47759e..0aa3e08 100644
--- a/lib/efi_loader/efi_memory.c
+++ b/lib/efi_loader/efi_memory.c
@@ -276,7 +276,7 @@ static uint64_t efi_find_free_memory(uint64_t len, uint64_t max_addr)
 }
 
 efi_status_t efi_allocate_pages(int type, int memory_type,
-				unsigned long pages, uint64_t *memory)
+				efi_uintn_t pages, uint64_t *memory)
 {
 	u64 len = pages << EFI_PAGE_SHIFT;
 	efi_status_t r = EFI_SUCCESS;
@@ -338,7 +338,7 @@ void *efi_alloc(uint64_t len, int memory_type)
 	return NULL;
 }
 
-efi_status_t efi_free_pages(uint64_t memory, unsigned long pages)
+efi_status_t efi_free_pages(uint64_t memory, efi_uintn_t pages)
 {
 	uint64_t r = 0;
 
@@ -351,7 +351,7 @@ efi_status_t efi_free_pages(uint64_t memory, unsigned long pages)
 	return EFI_NOT_FOUND;
 }
 
-efi_status_t efi_allocate_pool(int pool_type, unsigned long size,
+efi_status_t efi_allocate_pool(int pool_type, efi_uintn_t size,
 			       void **buffer)
 {
 	efi_status_t r;
@@ -392,16 +392,16 @@ efi_status_t efi_free_pool(void *buffer)
 	return r;
 }
 
-efi_status_t efi_get_memory_map(unsigned long *memory_map_size,
-			       struct efi_mem_desc *memory_map,
-			       unsigned long *map_key,
-			       unsigned long *descriptor_size,
-			       uint32_t *descriptor_version)
+efi_status_t efi_get_memory_map(efi_uintn_t *memory_map_size,
+				struct efi_mem_desc *memory_map,
+				efi_uintn_t *map_key,
+				efi_uintn_t *descriptor_size,
+				uint32_t *descriptor_version)
 {
-	ulong map_size = 0;
+	efi_uintn_t map_size = 0;
 	int map_entries = 0;
 	struct list_head *lhandle;
-	unsigned long provided_map_size = *memory_map_size;
+	efi_uintn_t provided_map_size = *memory_map_size;
 
 	list_for_each(lhandle, &efi_mem)
 		map_entries++;
diff --git a/lib/efi_loader/efi_net.c b/lib/efi_loader/efi_net.c
index 432d9a9..8c5d5b4 100644
--- a/lib/efi_loader/efi_net.c
+++ b/lib/efi_loader/efi_net.c
@@ -292,16 +292,25 @@ int efi_net_register(void)
 
 	/* We only expose the "active" eth device, so one is enough */
 	netobj = calloc(1, sizeof(*netobj));
+	if (!netobj)
+		goto out_of_memory;
+
+	/* Hook net up to the device list */
+	efi_add_handle(&netobj->parent);
 
 	/* Fill in object data */
-	netobj->parent.protocols[0].guid = &efi_net_guid;
-	netobj->parent.protocols[0].protocol_interface = &netobj->net;
-	netobj->parent.protocols[1].guid = &efi_guid_device_path;
-	netobj->parent.protocols[1].protocol_interface =
-		efi_dp_from_eth();
-	netobj->parent.protocols[2].guid = &efi_pxe_guid;
-	netobj->parent.protocols[2].protocol_interface = &netobj->pxe;
-	netobj->parent.handle = &netobj->net;
+	r = efi_add_protocol(netobj->parent.handle, &efi_net_guid,
+			     &netobj->net);
+	if (r != EFI_SUCCESS)
+		goto out_of_memory;
+	r = efi_add_protocol(netobj->parent.handle, &efi_guid_device_path,
+			     efi_dp_from_eth());
+	if (r != EFI_SUCCESS)
+		goto out_of_memory;
+	r = efi_add_protocol(netobj->parent.handle, &efi_pxe_guid,
+			     &netobj->pxe);
+	if (r != EFI_SUCCESS)
+		goto out_of_memory;
 	netobj->net.revision = EFI_SIMPLE_NETWORK_PROTOCOL_REVISION;
 	netobj->net.start = efi_net_start;
 	netobj->net.stop = efi_net_stop;
@@ -326,9 +335,6 @@ int efi_net_register(void)
 	if (dhcp_ack)
 		netobj->pxe_mode.dhcp_ack = *dhcp_ack;
 
-	/* Hook net up to the device list */
-	list_add_tail(&netobj->parent.link, &efi_obj_list);
-
 	/*
 	 * Create WaitForPacket event.
 	 */
@@ -361,4 +367,7 @@ int efi_net_register(void)
 	}
 
 	return 0;
+out_of_memory:
+	printf("ERROR: Out of memory\n");
+	return 1;
 }
diff --git a/lib/efi_loader/efi_watchdog.c b/lib/efi_loader/efi_watchdog.c
new file mode 100644
index 0000000..35a45de
--- /dev/null
+++ b/lib/efi_loader/efi_watchdog.c
@@ -0,0 +1,89 @@
+/*
+ *  EFI watchdog
+ *
+ *  Copyright (c) 2017 Heinrich Schuchardt
+ *
+ *  SPDX-License-Identifier:     GPL-2.0+
+ */
+
+#include <common.h>
+#include <efi_loader.h>
+
+/* Conversion factor from seconds to multiples of 100ns */
+#define EFI_SECONDS_TO_100NS 10000000ULL
+
+static struct efi_event *watchdog_timer_event;
+
+/*
+ * Reset the system when the watchdog event is notified.
+ *
+ * @event:	the watchdog event
+ * @context:	not used
+ */
+static void EFIAPI efi_watchdog_timer_notify(struct efi_event *event,
+					     void *context)
+{
+	EFI_ENTRY("%p, %p", event, context);
+
+	printf("\nEFI: Watchdog timeout\n");
+	EFI_CALL_VOID(efi_runtime_services.reset_system(EFI_RESET_COLD,
+							EFI_SUCCESS, 0, NULL));
+
+	EFI_EXIT(EFI_UNSUPPORTED);
+}
+
+/*
+ * Reset the watchdog timer.
+ *
+ * This function is used by the SetWatchdogTimer service.
+ *
+ * @timeout:		seconds before reset by watchdog
+ * @return:		status code
+ */
+efi_status_t efi_set_watchdog(unsigned long timeout)
+{
+	efi_status_t r;
+
+	if (timeout)
+		/* Reset watchdog */
+		r = efi_set_timer(watchdog_timer_event, EFI_TIMER_RELATIVE,
+				  EFI_SECONDS_TO_100NS * timeout);
+	else
+		/* Deactivate watchdog */
+		r = efi_set_timer(watchdog_timer_event, EFI_TIMER_STOP, 0);
+	return r;
+}
+
+/*
+ * Initialize the EFI watchdog.
+ *
+ * This function is called by efi_init_obj_list()
+ */
+int efi_watchdog_register(void)
+{
+	efi_status_t r;
+
+	/*
+	 * Create a timer event.
+	 */
+	r = efi_create_event(EVT_TIMER | EVT_NOTIFY_SIGNAL, TPL_CALLBACK,
+			     efi_watchdog_timer_notify, NULL,
+			     &watchdog_timer_event);
+	if (r != EFI_SUCCESS) {
+		printf("ERROR: Failed to register watchdog event\n");
+		return r;
+	}
+	/*
+	 * The UEFI standard requires that the watchdog timer is set to five
+	 * minutes when invoking an EFI boot option.
+	 *
+	 * Unified Extensible Firmware Interface (UEFI), version 2.7 Errata A
+	 * 7.5. Miscellaneous Boot Services - EFI_BOOT_SERVICES.SetWatchdogTimer
+	 */
+	r = efi_set_watchdog(300);
+	if (r != EFI_SUCCESS) {
+		printf("ERROR: Failed to set watchdog timer\n");
+		return r;
+	}
+	return 0;
+}
diff --git a/lib/efi_loader/helloworld.c b/lib/efi_loader/helloworld.c
index 03e65ab..e59c24c 100644
--- a/lib/efi_loader/helloworld.c
+++ b/lib/efi_loader/helloworld.c
@@ -5,20 +5,52 @@
  * Written by Simon Glass <sjg@chromium.org>
  *
  * SPDX-License-Identifier:     GPL-2.0+
+ *
+ * This program demonstrates calling a boottime service.
+ * It writes a greeting and the load options to the console.
  */
 
 #include <common.h>
-#include <part_efi.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;
 	struct efi_boot_services *boottime = systable->boottime;
+	struct efi_loaded_image *loaded_image;
+	const efi_guid_t loaded_image_guid = LOADED_IMAGE_GUID;
+	efi_status_t ret;
 
 	con_out->output_string(con_out, L"Hello, world!\n");
-	boottime->exit(handle, 0, 0, NULL);
 
-	return EFI_SUCCESS;
+	/* Get the loaded image protocol */
+	ret = boottime->handle_protocol(handle, &loaded_image_guid,
+					(void **)&loaded_image);
+	if (ret != EFI_SUCCESS) {
+		con_out->output_string(con_out,
+				       L"Cannot open loaded image protocol\n");
+		goto out;
+	}
+	/* Output the load options */
+	con_out->output_string(con_out, L"Load options: ");
+	if (loaded_image->load_options_size && loaded_image->load_options)
+		con_out->output_string(con_out,
+				       (u16 *)loaded_image->load_options);
+	else
+		con_out->output_string(con_out, L"<none>");
+	con_out->output_string(con_out, L"\n");
+
+out:
+	boottime->exit(handle, ret, 0, NULL);
+
+	/* We should never arrive here */
+	return ret;
 }
diff --git a/lib/efi_selftest/Makefile b/lib/efi_selftest/Makefile
index e446046..837e862 100644
--- a/lib/efi_selftest/Makefile
+++ b/lib/efi_selftest/Makefile
@@ -7,26 +7,16 @@
 # This file only gets included with CONFIG_EFI_LOADER set, so all
 # object inclusion implicitly depends on it
 
-CFLAGS_efi_selftest.o := $(CFLAGS_EFI)
-CFLAGS_REMOVE_efi_selftest.o := $(CFLAGS_NON_EFI)
-CFLAGS_efi_selftest_console.o := $(CFLAGS_EFI)
-CFLAGS_REMOVE_efi_selftest_console.o := $(CFLAGS_NON_EFI)
-CFLAGS_efi_selftest_events.o := $(CFLAGS_EFI)
-CFLAGS_REMOVE_efi_selftest_events.o := $(CFLAGS_NON_EFI)
-CFLAGS_efi_selftest_exitbootservices.o := $(CFLAGS_EFI)
-CFLAGS_REMOVE_efi_selftest_exitbootservices.o := $(CFLAGS_NON_EFI)
-CFLAGS_efi_selftest_snp.o := $(CFLAGS_EFI)
-CFLAGS_REMOVE_efi_selftest_snp.o := $(CFLAGS_NON_EFI)
-CFLAGS_efi_selftest_tpl.o := $(CFLAGS_EFI)
-CFLAGS_REMOVE_efi_selftest_tpl.o := $(CFLAGS_NON_EFI)
-CFLAGS_efi_selftest_util.o := $(CFLAGS_EFI)
-CFLAGS_REMOVE_efi_selftest_util.o := $(CFLAGS_NON_EFI)
-
 obj-$(CONFIG_CMD_BOOTEFI_SELFTEST) += \
 efi_selftest.o \
 efi_selftest_console.o \
+efi_selftest_devicepath.o \
 efi_selftest_events.o \
 efi_selftest_exitbootservices.o \
+efi_selftest_gop.o \
+efi_selftest_manageprotocols.o \
 efi_selftest_snp.o \
+efi_selftest_textoutput.o \
 efi_selftest_tpl.o \
-efi_selftest_util.o
+efi_selftest_util.o \
+efi_selftest_watchdog.o
diff --git a/lib/efi_selftest/efi_selftest.c b/lib/efi_selftest/efi_selftest.c
index 45d8d3d..4e5a12c 100644
--- a/lib/efi_selftest/efi_selftest.c
+++ b/lib/efi_selftest/efi_selftest.c
@@ -9,6 +9,13 @@
 #include <efi_selftest.h>
 #include <vsprintf.h>
 
+/*
+ * Constants for test step bitmap
+ */
+#define EFI_ST_SETUP	1
+#define EFI_ST_EXECUTE	2
+#define EFI_ST_TEARDOWN	4
+
 static const struct efi_system_table *systable;
 static const struct efi_boot_services *boottime;
 static const struct efi_runtime_services *runtime;
@@ -25,9 +32,9 @@ static u16 reset_message[] = L"Selftest completed";
  */
 void efi_st_exit_boot_services(void)
 {
-	unsigned long  map_size = 0;
-	unsigned long  map_key;
-	unsigned long desc_size;
+	efi_uintn_t map_size = 0;
+	efi_uintn_t map_key;
+	efi_uintn_t desc_size;
 	u32 desc_version;
 	efi_status_t ret;
 	struct efi_mem_desc *memory_map;
@@ -134,6 +141,70 @@ static int teardown(struct efi_unit_test *test, unsigned int *failures)
 }
 
 /*
+ * Check that a test exists.
+ *
+ * @testname:	name of the test
+ * @return:	test
+ */
+static struct efi_unit_test *find_test(const u16 *testname)
+{
+	struct efi_unit_test *test;
+
+	for (test = ll_entry_start(struct efi_unit_test, efi_unit_test);
+	     test < ll_entry_end(struct efi_unit_test, efi_unit_test); ++test) {
+		if (!efi_st_strcmp_16_8(testname, test->name))
+			return test;
+	}
+	efi_st_printf("\nTest '%ps' not found\n", testname);
+	return NULL;
+}
+
+/*
+ * List all available tests.
+ */
+static void list_all_tests(void)
+{
+	struct efi_unit_test *test;
+
+	/* List all tests */
+	efi_st_printf("\nAvailable tests:\n");
+	for (test = ll_entry_start(struct efi_unit_test, efi_unit_test);
+	     test < ll_entry_end(struct efi_unit_test, efi_unit_test); ++test) {
+		efi_st_printf("'%s'%s\n", test->name,
+			      test->on_request ? " - on request" : "");
+	}
+}
+
+/*
+ * Execute test steps of one phase.
+ *
+ * @testname	name of a single selected test or NULL
+ * @phase	test phase
+ * @steps	steps to execute
+ * failures	returns EFI_ST_SUCCESS if all test steps succeeded
+ */
+void efi_st_do_tests(const u16 *testname, unsigned int phase,
+		     unsigned int steps, unsigned int *failures)
+{
+	struct efi_unit_test *test;
+
+	for (test = ll_entry_start(struct efi_unit_test, efi_unit_test);
+	     test < ll_entry_end(struct efi_unit_test, efi_unit_test); ++test) {
+		if (testname ?
+		    efi_st_strcmp_16_8(testname, test->name) : test->on_request)
+			continue;
+		if (test->phase != phase)
+			continue;
+		if (steps & EFI_ST_SETUP)
+			setup(test, failures);
+		if (steps & EFI_ST_EXECUTE)
+			execute(test, failures);
+		if (steps & EFI_ST_TEARDOWN)
+			teardown(test, failures);
+	}
+}
+
+/*
  * Execute selftest of the EFI API
  *
  * This is the main entry point of the EFI selftest application.
@@ -153,8 +224,10 @@ static int teardown(struct efi_unit_test *test, unsigned int *failures)
 efi_status_t EFIAPI efi_selftest(efi_handle_t image_handle,
 				 struct efi_system_table *systab)
 {
-	struct efi_unit_test *test;
 	unsigned int failures = 0;
+	const u16 *testname = NULL;
+	struct efi_loaded_image *loaded_image;
+	efi_status_t ret;
 
 	systable = systab;
 	boottime = systable->boottime;
@@ -163,47 +236,59 @@ efi_status_t EFIAPI efi_selftest(efi_handle_t image_handle,
 	con_out = systable->con_out;
 	con_in = systable->con_in;
 
-	efi_st_printf("\nTesting EFI API implementation\n");
+	ret = boottime->handle_protocol(image_handle, &efi_guid_loaded_image,
+					(void **)&loaded_image);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Cannot open loaded image protocol\n");
+		return ret;
+	}
 
-	efi_st_printf("\nNumber of tests to execute: %u\n",
-		      ll_entry_count(struct efi_unit_test, efi_unit_test));
+	if (loaded_image->load_options)
+		testname = (u16 *)loaded_image->load_options;
 
-	/* Execute boottime tests */
-	for (test = ll_entry_start(struct efi_unit_test, efi_unit_test);
-	     test < ll_entry_end(struct efi_unit_test, efi_unit_test); ++test) {
-		if (test->phase == EFI_EXECUTE_BEFORE_BOOTTIME_EXIT) {
-			setup(test, &failures);
-			execute(test, &failures);
-			teardown(test, &failures);
+	if (testname) {
+		if (!efi_st_strcmp_16_8(testname, "list") ||
+		    !find_test(testname)) {
+			list_all_tests();
+			/*
+			 * TODO:
+			 * Once the Exit boottime service is correctly
+			 * implemented we should call
+			 *   boottime->exit(image_handle, EFI_SUCCESS, 0, NULL);
+			 * here, cf.
+			 * https://lists.denx.de/pipermail/u-boot/2017-October/308720.html
+			 */
+			return EFI_SUCCESS;
 		}
 	}
 
+	efi_st_printf("\nTesting EFI API implementation\n");
+
+	if (testname)
+		efi_st_printf("\nSelected test: '%ps'\n", testname);
+	else
+		efi_st_printf("\nNumber of tests to execute: %u\n",
+			      ll_entry_count(struct efi_unit_test,
+					     efi_unit_test));
+
+	/* Execute boottime tests */
+	efi_st_do_tests(testname, EFI_EXECUTE_BEFORE_BOOTTIME_EXIT,
+			EFI_ST_SETUP | EFI_ST_EXECUTE | EFI_ST_TEARDOWN,
+			&failures);
+
 	/* Execute mixed tests */
-	for (test = ll_entry_start(struct efi_unit_test, efi_unit_test);
-	     test < ll_entry_end(struct efi_unit_test, efi_unit_test); ++test) {
-		if (test->phase == EFI_SETUP_BEFORE_BOOTTIME_EXIT)
-			setup(test, &failures);
-	}
+	efi_st_do_tests(testname, EFI_SETUP_BEFORE_BOOTTIME_EXIT,
+			EFI_ST_SETUP, &failures);
 
 	efi_st_exit_boot_services();
 
-	for (test = ll_entry_start(struct efi_unit_test, efi_unit_test);
-	     test < ll_entry_end(struct efi_unit_test, efi_unit_test); ++test) {
-		if (test->phase == EFI_SETUP_BEFORE_BOOTTIME_EXIT) {
-			execute(test, &failures);
-			teardown(test, &failures);
-		}
-	}
+	efi_st_do_tests(testname, EFI_SETUP_BEFORE_BOOTTIME_EXIT,
+			EFI_ST_EXECUTE | EFI_ST_TEARDOWN, &failures);
 
 	/* Execute runtime tests */
-	for (test = ll_entry_start(struct efi_unit_test, efi_unit_test);
-	     test < ll_entry_end(struct efi_unit_test, efi_unit_test); ++test) {
-		if (test->phase == EFI_SETUP_AFTER_BOOTTIME_EXIT) {
-			setup(test, &failures);
-			execute(test, &failures);
-			teardown(test, &failures);
-		}
-	}
+	efi_st_do_tests(testname, EFI_SETUP_AFTER_BOOTTIME_EXIT,
+			EFI_ST_SETUP | EFI_ST_EXECUTE | EFI_ST_TEARDOWN,
+			&failures);
 
 	/* Give feedback */
 	efi_st_printf("\nSummary: %u failures\n\n", failures);
diff --git a/lib/efi_selftest/efi_selftest_console.c b/lib/efi_selftest/efi_selftest_console.c
index 840e229..6a7fd20 100644
--- a/lib/efi_selftest/efi_selftest_console.c
+++ b/lib/efi_selftest/efi_selftest_console.c
@@ -142,6 +142,7 @@ void efi_st_printf(const char *fmt, ...)
 	const char *c;
 	u16 *pos = buf;
 	const char *s;
+	const u16 *u;
 
 	va_start(args, fmt);
 
@@ -179,9 +180,18 @@ void efi_st_printf(const char *fmt, ...)
 			case 'p':
 				++c;
 				switch (*c) {
+				/* MAC address */
 				case 'm':
 					mac(va_arg(args, void*), &pos);
 					break;
+
+				/* u16 string */
+				case 's':
+					u = va_arg(args, u16*);
+					/* Ensure string fits into buffer */
+					for (; *u && pos < buf + 120; ++u)
+						*pos++ = *u;
+					break;
 				default:
 					--c;
 					pointer(va_arg(args, void*), &pos);
diff --git a/lib/efi_selftest/efi_selftest_devicepath.c b/lib/efi_selftest/efi_selftest_devicepath.c
new file mode 100644
index 0000000..1ab54eb
--- /dev/null
+++ b/lib/efi_selftest/efi_selftest_devicepath.c
@@ -0,0 +1,390 @@
+/*
+ * efi_selftest_devicepath
+ *
+ * Copyright (c) 2017 Heinrich Schuchardt <xypron.glpk@gmx.de>
+ *
+ * SPDX-License-Identifier:     GPL-2.0+
+ *
+ * This unit test checks the following protocol services:
+ * DevicePathToText
+ */
+
+#include <efi_selftest.h>
+
+static struct efi_boot_services *boottime;
+
+static efi_handle_t handle1;
+static efi_handle_t handle2;
+static efi_handle_t handle3;
+
+struct interface {
+	void (EFIAPI * inc)(void);
+} interface;
+
+static efi_guid_t guid_device_path = DEVICE_PATH_GUID;
+
+static efi_guid_t guid_device_path_to_text_protocol =
+	EFI_DEVICE_PATH_TO_TEXT_PROTOCOL_GUID;
+
+static efi_guid_t guid_protocol =
+	EFI_GUID(0xdbca4c98, 0x6cb0, 0x694d,
+		 0x08, 0x72, 0x81, 0x9c, 0x65, 0x0c, 0xbb, 0x7d);
+
+static efi_guid_t guid_vendor1 =
+	EFI_GUID(0xdbca4c98, 0x6cb0, 0x694d,
+		 0x08, 0x72, 0x81, 0x9c, 0x65, 0x0c, 0xbb, 0xb1);
+
+static efi_guid_t guid_vendor2 =
+	EFI_GUID(0xdbca4c98, 0x6cb0, 0x694d,
+		 0x08, 0x72, 0x81, 0x9c, 0x65, 0x0c, 0xbb, 0xa2);
+
+static efi_guid_t guid_vendor3 =
+	EFI_GUID(0xdbca4c98, 0x6cb0, 0x694d,
+		 0x08, 0x72, 0x81, 0x9c, 0x65, 0x0c, 0xbb, 0xc3);
+
+static u8 *dp1;
+static u8 *dp2;
+static u8 *dp3;
+
+struct efi_device_path_to_text_protocol *device_path_to_text;
+
+/*
+ * Setup unit test.
+ *
+ * Create three handles. Install a new protocol on two of them and
+ * provice device paths.
+ *
+ * handle1
+ *   guid interface
+ * handle2
+ *   guid interface
+ * handle3
+ *
+ * @handle:	handle of the loaded image
+ * @systable:	system table
+ */
+static int setup(const efi_handle_t img_handle,
+		 const struct efi_system_table *systable)
+{
+	struct efi_device_path_vendor vendor_node;
+	struct efi_device_path end_node;
+	efi_status_t ret;
+
+	boottime = systable->boottime;
+
+	ret = boottime->locate_protocol(&guid_device_path_to_text_protocol,
+					NULL, (void **)&device_path_to_text);
+	if (ret != EFI_SUCCESS) {
+		device_path_to_text = NULL;
+		efi_st_error(
+			"Device path to text protocol is not available.\n");
+		return EFI_ST_FAILURE;
+	}
+
+	ret = boottime->allocate_pool(EFI_LOADER_DATA,
+				      sizeof(struct efi_device_path_vendor) +
+				      sizeof(struct efi_device_path),
+				      (void **)&dp1);
+	if (ret != EFI_SUCCESS)
+		goto out_of_memory;
+
+	ret = boottime->allocate_pool(EFI_LOADER_DATA, 2 *
+				      sizeof(struct efi_device_path_vendor) +
+				      sizeof(struct efi_device_path),
+				      (void **)&dp2);
+	if (ret != EFI_SUCCESS)
+		goto out_of_memory;
+
+	ret = boottime->allocate_pool(EFI_LOADER_DATA, 3 *
+				      sizeof(struct efi_device_path_vendor) +
+				      sizeof(struct efi_device_path),
+				      (void **)&dp3);
+	if (ret != EFI_SUCCESS)
+		goto out_of_memory;
+
+	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_vendor1,
+			   sizeof(efi_guid_t));
+	boottime->copy_mem(dp1, &vendor_node,
+			   sizeof(struct efi_device_path_vendor));
+	boottime->copy_mem(dp2, &vendor_node,
+			   sizeof(struct efi_device_path_vendor));
+	boottime->copy_mem(dp3, &vendor_node,
+			   sizeof(struct efi_device_path_vendor));
+
+	boottime->copy_mem(&vendor_node.guid, &guid_vendor2,
+			   sizeof(efi_guid_t));
+	boottime->copy_mem(dp2 + sizeof(struct efi_device_path_vendor),
+			   &vendor_node, sizeof(struct efi_device_path_vendor));
+	boottime->copy_mem(dp3 + sizeof(struct efi_device_path_vendor),
+			   &vendor_node, sizeof(struct efi_device_path_vendor));
+
+	boottime->copy_mem(&vendor_node.guid, &guid_vendor3,
+			   sizeof(efi_guid_t));
+	boottime->copy_mem(dp3 + 2 * sizeof(struct efi_device_path_vendor),
+			   &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(dp1 + sizeof(struct efi_device_path_vendor),
+			   &end_node, sizeof(struct efi_device_path));
+	boottime->copy_mem(dp2 + 2 * sizeof(struct efi_device_path_vendor),
+			   &end_node, sizeof(struct efi_device_path));
+	boottime->copy_mem(dp3 + 3 * sizeof(struct efi_device_path_vendor),
+			   &end_node, sizeof(struct efi_device_path));
+
+	ret = boottime->install_protocol_interface(&handle1,
+						   &guid_device_path,
+						   EFI_NATIVE_INTERFACE,
+						   dp1);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("InstallProtocolInterface failed\n");
+		return EFI_ST_FAILURE;
+	}
+	ret = boottime->install_protocol_interface(&handle1,
+						   &guid_protocol,
+						   EFI_NATIVE_INTERFACE,
+						   &interface);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("InstallProtocolInterface failed\n");
+		return EFI_ST_FAILURE;
+	}
+	ret = boottime->install_protocol_interface(&handle2,
+						   &guid_device_path,
+						   EFI_NATIVE_INTERFACE,
+						   dp2);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("InstallProtocolInterface failed\n");
+		return EFI_ST_FAILURE;
+	}
+	ret = boottime->install_protocol_interface(&handle2,
+						   &guid_protocol,
+						   EFI_NATIVE_INTERFACE,
+						   &interface);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("InstallProtocolInterface failed\n");
+		return EFI_ST_FAILURE;
+	}
+	ret = boottime->install_protocol_interface(&handle3,
+						   &guid_device_path,
+						   EFI_NATIVE_INTERFACE,
+						   dp3);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("InstallProtocolInterface failed\n");
+		return EFI_ST_FAILURE;
+	}
+	return EFI_ST_SUCCESS;
+
+out_of_memory:
+	efi_st_error("Out of memory\n");
+	return EFI_ST_FAILURE;
+}
+
+/*
+ * Tear down unit test.
+ *
+ */
+static int teardown(void)
+{
+	efi_status_t ret;
+
+	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,
+						     &guid_protocol,
+						     &interface);
+	if (ret != EFI_SUCCESS)
+		efi_st_todo("UninstallProtocolInterface failed\n");
+	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,
+						     &guid_protocol,
+						     &interface);
+	if (ret != EFI_SUCCESS)
+		efi_st_todo("UninstallProtocolInterface failed\n");
+	ret = boottime->uninstall_protocol_interface(&handle3,
+						     &guid_device_path,
+						     dp3);
+	if (ret != EFI_SUCCESS)
+		efi_st_todo("UninstallProtocolInterface failed\n");
+	if (dp1) {
+		ret = boottime->free_pool(dp1);
+		if (ret != EFI_SUCCESS) {
+			efi_st_error("FreePool failed\n");
+			return EFI_ST_FAILURE;
+		}
+	}
+	if (dp2) {
+		ret = boottime->free_pool(dp2);
+		if (ret != EFI_SUCCESS) {
+			efi_st_error("FreePool failed\n");
+			return EFI_ST_FAILURE;
+		}
+	}
+	if (dp3) {
+		ret = boottime->free_pool(dp3);
+		if (ret != EFI_SUCCESS) {
+			efi_st_error("FreePool failed\n");
+			return EFI_ST_FAILURE;
+		}
+	}
+	return EFI_ST_SUCCESS;
+}
+
+/*
+ * Execute unit test.
+ *
+ */
+static int execute(void)
+{
+	struct efi_device_path *remaining_dp;
+	void *handle;
+	/*
+	 * This device path node ends with the letter 't' of 'u-boot'.
+	 * The following '.bin' does not belong to the node but is
+	 * helps to test the correct truncation.
+	 */
+	struct {
+		struct efi_device_path dp;
+		u16 text[12];
+	} __packed dp_node = {
+			{ DEVICE_PATH_TYPE_MEDIA_DEVICE,
+			  DEVICE_PATH_SUB_TYPE_FILE_PATH,
+			  sizeof(struct efi_device_path) + 12},
+			L"u-boot.bin",
+		};
+	u16 *string;
+	efi_status_t ret;
+	efi_uintn_t i, no_handles;
+	efi_handle_t *handles;
+	struct efi_device_path *dp;
+
+	/* Display all available device paths */
+	ret = boottime->locate_handle_buffer(BY_PROTOCOL,
+					     &guid_device_path,
+					     NULL, &no_handles, &handles);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Cannot retrieve device path protocols.\n");
+		return EFI_ST_FAILURE;
+	}
+
+	efi_st_printf("Installed device path protocols:\n");
+	for (i = 0; i < no_handles; ++i) {
+		ret = boottime->open_protocol(handles[i], &guid_device_path,
+					      (void **)&dp, NULL, NULL,
+					      EFI_OPEN_PROTOCOL_GET_PROTOCOL);
+		if (ret != EFI_SUCCESS) {
+			efi_st_error("Cannot open device path protocol.\n");
+			return EFI_ST_FAILURE;
+		}
+		string = device_path_to_text->convert_device_path_to_text(
+					dp, true, false);
+		if (!string) {
+			efi_st_error("ConvertDevicePathToText failed\n");
+			return EFI_ST_FAILURE;
+		}
+		efi_st_printf("%ps\n", string);
+		ret = boottime->free_pool(string);
+		if (ret != EFI_SUCCESS) {
+			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");
+	}
+	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(
+			(struct efi_device_path *)dp2, true, false);
+	if (!string) {
+		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_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");
+		return EFI_ST_FAILURE;
+	}
+
+	/* Test ConvertDeviceNodeToText */
+	string = device_path_to_text->convert_device_node_to_text(
+			(struct efi_device_path *)&dp_node, true, false);
+	if (!string) {
+		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_error(
+			"Incorrect conversion by ConvertDeviceNodeToText\n");
+		return EFI_ST_FAILURE;
+	}
+
+	/* Test LocateDevicePath */
+	remaining_dp = (struct efi_device_path *)dp3;
+	ret = boottime->locate_device_path(&guid_protocol, &remaining_dp,
+					   &handle);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("LocateDevicePath failed\n");
+		return EFI_ST_FAILURE;
+	}
+	if (handle != handle2) {
+		efi_st_error("LocateDevicePath returned wrong handle\n");
+		return EFI_ST_FAILURE;
+	}
+	string = device_path_to_text->convert_device_path_to_text(remaining_dp,
+								  true, false);
+	if (!string) {
+		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_error("LocateDevicePath: wrong remaining device path\n");
+		return EFI_ST_FAILURE;
+	}
+
+	return EFI_ST_SUCCESS;
+}
+
+EFI_UNIT_TEST(devicepath) = {
+	.name = "device path",
+	.phase = EFI_EXECUTE_BEFORE_BOOTTIME_EXIT,
+	.setup = setup,
+	.execute = execute,
+	.teardown = teardown,
+};
diff --git a/lib/efi_selftest/efi_selftest_events.c b/lib/efi_selftest/efi_selftest_events.c
index 081f312..ad9490b 100644
--- a/lib/efi_selftest/efi_selftest_events.c
+++ b/lib/efi_selftest/efi_selftest_events.c
@@ -108,7 +108,7 @@ static int teardown(void)
  */
 static int execute(void)
 {
-	size_t index;
+	efi_uintn_t index;
 	efi_status_t ret;
 
 	/* Set 10 ms timer */
diff --git a/lib/efi_selftest/efi_selftest_gop.c b/lib/efi_selftest/efi_selftest_gop.c
new file mode 100644
index 0000000..2a0553b
--- /dev/null
+++ b/lib/efi_selftest/efi_selftest_gop.c
@@ -0,0 +1,95 @@
+/*
+ * efi_selftest_gop
+ *
+ * Copyright (c) 2017 Heinrich Schuchardt <xypron.glpk@gmx.de>
+ *
+ * SPDX-License-Identifier:     GPL-2.0+
+ *
+ * Test the graphical output protocol.
+ */
+
+#include <efi_selftest.h>
+
+static struct efi_boot_services *boottime;
+static efi_guid_t efi_gop_guid = EFI_GOP_GUID;
+static struct efi_gop *gop;
+
+/*
+ * 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;
+
+	boottime = systable->boottime;
+
+	ret = boottime->locate_protocol(&efi_gop_guid, NULL, (void **)&gop);
+	if (ret != EFI_SUCCESS) {
+		gop = NULL;
+		efi_st_printf("Graphical output protocol is not available.\n");
+	}
+
+	return EFI_ST_SUCCESS;
+}
+
+/*
+ * Tear down unit test.
+ *
+ * @return:	EFI_ST_SUCCESS for success
+ */
+static int teardown(void)
+{
+	return EFI_ST_SUCCESS;
+}
+
+/*
+ * Execute unit test.
+ *
+ * @return:	EFI_ST_SUCCESS for success
+ */
+static int execute(void)
+{
+	efi_status_t ret;
+	u32 i, max_mode;
+	efi_uintn_t size;
+	struct efi_gop_mode_info *info;
+
+	if (!gop)
+		return EFI_ST_SUCCESS;
+
+	if (!gop->mode) {
+		efi_st_error("EFI_GRAPHICS_OUTPUT_PROTOCOL_MODE missing\n");
+		return EFI_ST_FAILURE;
+	}
+	max_mode = gop->mode->max_mode;
+	if (!max_mode) {
+		efi_st_error("No graphical mode available\n");
+		return EFI_ST_FAILURE;
+	}
+	efi_st_printf("Number of available modes: %u\n", max_mode);
+
+	for (i = 0; i < max_mode; ++i) {
+		ret = gop->query_mode(gop, i, &size, &info);
+		if (ret != EFI_SUCCESS) {
+			efi_st_printf("Could not query mode %u\n", i);
+			return EFI_ST_FAILURE;
+		}
+		efi_st_printf("Mode %u: %u x %u\n",
+			      i, info->width, info->height);
+	}
+
+	return EFI_ST_SUCCESS;
+}
+
+EFI_UNIT_TEST(gop) = {
+	.name = "graphical output",
+	.phase = EFI_EXECUTE_BEFORE_BOOTTIME_EXIT,
+	.setup = setup,
+	.execute = execute,
+	.teardown = teardown,
+};
diff --git a/lib/efi_selftest/efi_selftest_manageprotocols.c b/lib/efi_selftest/efi_selftest_manageprotocols.c
new file mode 100644
index 0000000..f20f152
--- /dev/null
+++ b/lib/efi_selftest/efi_selftest_manageprotocols.c
@@ -0,0 +1,354 @@
+/*
+ * efi_selftest_manageprotocols
+ *
+ * Copyright (c) 2017 Heinrich Schuchardt <xypron.glpk@gmx.de>
+ *
+ * SPDX-License-Identifier:     GPL-2.0+
+ *
+ * This unit test checks the following protocol services:
+ * InstallProtocolInterface, UninstallProtocolInterface,
+ * InstallMultipleProtocolsInterfaces, UninstallMultipleProtocolsInterfaces,
+ * HandleProtocol, ProtocolsPerHandle,
+ * LocateHandle, LocateHandleBuffer.
+ */
+
+#include <efi_selftest.h>
+
+/*
+ * The test currently does not actually call the interface function.
+ * So this is just a dummy structure.
+ */
+struct interface {
+	void (EFIAPI * inc)(void);
+};
+
+static struct efi_boot_services *boottime;
+static efi_guid_t guid1 =
+	EFI_GUID(0x2e7ca819, 0x21d3, 0x0a3a,
+		 0xf7, 0x91, 0x82, 0x1f, 0x7a, 0x83, 0x67, 0xaf);
+static efi_guid_t guid2 =
+	EFI_GUID(0xf909f2bb, 0x90a8, 0x0d77,
+		 0x94, 0x0c, 0x3e, 0xa8, 0xea, 0x38, 0xd6, 0x6f);
+static efi_guid_t guid3 =
+	EFI_GUID(0x06d641a3, 0xf4e7, 0xe0c9,
+		 0xe7, 0x8d, 0x41, 0x2d, 0x72, 0xa6, 0xb1, 0x24);
+static efi_handle_t handle1;
+static efi_handle_t handle2;
+static struct interface interface1;
+static struct interface interface2;
+static struct interface interface3;
+static struct interface interface4;
+
+/*
+ * Find a handle in an array.
+ *
+ * @handle:	handle to find
+ * @count:	number of entries in the array
+ * @buffer:	array to search
+ */
+efi_status_t find_in_buffer(efi_handle_t handle, size_t count,
+			    efi_handle_t *buffer)
+{
+	size_t i;
+
+	for (i = 0; i < count; ++i) {
+		if (buffer[i] == handle)
+			return EFI_SUCCESS;
+	}
+	return EFI_NOT_FOUND;
+}
+
+/*
+ * Setup unit test.
+ *
+ * Create two handles and install two out of three protocol interfaces on each
+ * of them:
+ *
+ * handle1
+ *   guid1 interface1
+ *   guid3 interface3
+ * handle2
+ *   guid1 interface4
+ *   guid2 interface2
+ *
+ * @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;
+	efi_handle_t handle;
+
+	boottime = systable->boottime;
+
+	ret = boottime->install_protocol_interface(&handle1, &guid3,
+						   EFI_NATIVE_INTERFACE,
+						   &interface3);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("InstallProtocolInterface failed\n");
+		return EFI_ST_FAILURE;
+	}
+	if (!handle1) {
+		efi_st_error("InstallProtocolInterface failed to create handle\n");
+		return EFI_ST_FAILURE;
+	}
+	handle = handle1;
+	ret = boottime->install_protocol_interface(&handle1, &guid1,
+						   EFI_NATIVE_INTERFACE,
+						   &interface1);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("InstallProtocolInterface failed\n");
+		return EFI_ST_FAILURE;
+	}
+	if (handle != handle1) {
+		efi_st_error("InstallProtocolInterface failed to use handle\n");
+		return EFI_ST_FAILURE;
+	}
+	ret = boottime->install_multiple_protocol_interfaces(&handle2,
+			&guid1, &interface4, &guid2, &interface2, NULL);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("InstallMultipleProtocolInterfaces failed\n");
+		return EFI_ST_FAILURE;
+	}
+	if (!handle2 || handle1 == handle2) {
+		efi_st_error("InstallMultipleProtocolInterfaces failed to create handle\n");
+		return EFI_ST_FAILURE;
+	}
+
+	return EFI_ST_SUCCESS;
+}
+
+/*
+ * Tear down unit test.
+ *
+ */
+static int teardown(void)
+{
+	return EFI_ST_SUCCESS;
+}
+
+/*
+ * Execute unit test.
+ *
+ */
+static int execute(void)
+{
+	struct interface *interface;
+	efi_status_t ret;
+	efi_handle_t *buffer;
+	size_t buffer_size;
+	efi_uintn_t count = 0;
+	efi_guid_t **prot_buffer;
+	efi_uintn_t prot_count;
+
+	/*
+	 * Test HandleProtocol
+	 */
+	ret = boottime->handle_protocol(handle1, &guid3, (void **)&interface);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("HandleProtocol failed to retrieve interface\n");
+		return EFI_ST_FAILURE;
+	}
+	if (interface != &interface3) {
+		efi_st_error("HandleProtocol returned wrong interface\n");
+		return EFI_ST_FAILURE;
+	}
+	ret = boottime->handle_protocol(handle1, &guid2, (void **)&interface);
+	if (ret == EFI_SUCCESS) {
+		efi_st_error("HandleProtocol returned not installed interface\n");
+		return EFI_ST_FAILURE;
+	}
+
+	/*
+	 * Test LocateHandleBuffer with AllHandles
+	 */
+	ret = boottime->locate_handle_buffer(ALL_HANDLES, NULL, NULL,
+					     &count, &buffer);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("LocateHandleBuffer with AllHandles failed\n");
+		return EFI_ST_FAILURE;
+	}
+	buffer_size = count;
+	ret = find_in_buffer(handle1, count, buffer);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("LocateHandleBuffer failed to locate new handle\n");
+		return EFI_ST_FAILURE;
+	}
+	ret = find_in_buffer(handle2, count, buffer);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("LocateHandleBuffer failed to locate new handle\n");
+		return EFI_ST_FAILURE;
+	}
+	boottime->set_mem(buffer, sizeof(efi_handle_t) * buffer_size, 0);
+
+	/*
+	 * Test error handling in UninstallMultipleProtocols
+	 *
+	 * Try to uninstall more protocols than there are installed.
+	 */
+	ret = boottime->uninstall_multiple_protocol_interfaces(
+						handle2,
+						&guid1, &interface4,
+						&guid2, &interface2,
+						&guid3, &interface3,
+						NULL);
+	if (ret == EFI_SUCCESS) {
+		efi_st_todo("UninstallMultipleProtocolInterfaces did not catch error\n");
+		return EFI_ST_FAILURE;
+	}
+
+	/*
+	 * Test LocateHandleBuffer with ByProtocol
+	 */
+	count = buffer_size;
+	ret = boottime->locate_handle_buffer(BY_PROTOCOL, &guid1, NULL,
+					     &count, &buffer);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("LocateHandleBuffer failed to locate new handles\n");
+		return EFI_ST_FAILURE;
+	}
+	if (count != 2) {
+		efi_st_error("LocateHandleBuffer failed to locate new handles\n");
+		return EFI_ST_FAILURE;
+	}
+	ret = find_in_buffer(handle1, count, buffer);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("LocateHandleBuffer failed to locate new handle\n");
+		return EFI_ST_FAILURE;
+	}
+	ret = find_in_buffer(handle2, count, buffer);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("LocateHandleBuffer failed to locate new handle\n");
+		return EFI_ST_FAILURE;
+	}
+	boottime->set_mem(buffer, sizeof(efi_handle_t) * buffer_size, 0);
+
+	/*
+	 * Test LocateHandle with ByProtocol
+	 */
+	count = buffer_size * sizeof(efi_handle_t);
+	ret = boottime->locate_handle(BY_PROTOCOL, &guid1, NULL,
+				      &count, buffer);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("LocateHandle with ByProtocol failed\n");
+		return EFI_ST_FAILURE;
+	}
+	if (count / sizeof(efi_handle_t) != 2) {
+		efi_st_error("LocateHandle failed to locate new handles\n");
+		return EFI_ST_FAILURE;
+	}
+	buffer_size = count;
+	ret = find_in_buffer(handle1, count, buffer);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("LocateHandle failed to locate new handles\n");
+		return EFI_ST_FAILURE;
+	}
+	ret = find_in_buffer(handle2, count, buffer);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("LocateHandle failed to locate new handles\n");
+		return EFI_ST_FAILURE;
+	}
+	boottime->set_mem(buffer, sizeof(efi_handle_t) * buffer_size, 0);
+
+	/*
+	 * Test LocateProtocol
+	 */
+	ret = boottime->locate_protocol(&guid1, NULL, (void **)&interface);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("LocateProtocol failed\n");
+		return EFI_ST_FAILURE;
+	}
+	if (interface != &interface1 && interface != &interface4) {
+		efi_st_error("LocateProtocol failed to locate protocol\n");
+		return EFI_ST_FAILURE;
+	}
+
+	/*
+	 * Test UninstallMultipleProtocols
+	 */
+	ret = boottime->uninstall_multiple_protocol_interfaces(
+						handle2,
+						&guid1, &interface4,
+						&guid2, &interface2,
+						NULL);
+	if (ret != EFI_SUCCESS) {
+		efi_st_todo("UninstallMultipleProtocolInterfaces failed\n");
+		/* This test is known to fail due to missing implementation */
+	}
+	/*
+	 * Check that the protocols are really uninstalled.
+	 */
+	count = buffer_size;
+	ret = boottime->locate_handle_buffer(BY_PROTOCOL, &guid1, NULL,
+					     &count, &buffer);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("LocateHandleBuffer failed\n");
+		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 */
+	}
+	ret = find_in_buffer(handle1, count, buffer);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Failed to locate new handle\n");
+		return EFI_ST_FAILURE;
+	}
+	boottime->set_mem(buffer, sizeof(efi_handle_t) * buffer_size, 0);
+
+	/*
+	 * Test ProtocolsPerHandle
+	 */
+	ret = boottime->protocols_per_handle(handle1,
+					     &prot_buffer, &prot_count);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Failed to get protocols per handle\n");
+		return EFI_ST_FAILURE;
+	}
+	if (prot_count != 2) {
+		efi_st_error("Failed to get protocols per handle\n");
+		return EFI_ST_FAILURE;
+	}
+	if (efi_st_memcmp(prot_buffer[0], &guid1, 16) &&
+	    efi_st_memcmp(prot_buffer[1], &guid1, 16)) {
+		efi_st_error("Failed to get protocols per handle\n");
+		return EFI_ST_FAILURE;
+	}
+	if (efi_st_memcmp(prot_buffer[0], &guid3, 16) &&
+	    efi_st_memcmp(prot_buffer[1], &guid3, 16)) {
+		efi_st_error("Failed to get protocols per handle\n");
+		return EFI_ST_FAILURE;
+	}
+
+	/*
+	 * Uninstall remaining protocols
+	 */
+	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 */
+	}
+	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 */
+	}
+	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 */
+	}
+
+	return EFI_ST_SUCCESS;
+}
+
+EFI_UNIT_TEST(protserv) = {
+	.name = "manage protocols",
+	.phase = EFI_EXECUTE_BEFORE_BOOTTIME_EXIT,
+	.setup = setup,
+	.execute = execute,
+	.teardown = teardown,
+};
diff --git a/lib/efi_selftest/efi_selftest_snp.c b/lib/efi_selftest/efi_selftest_snp.c
index bdd6ce2..cc0705f 100644
--- a/lib/efi_selftest/efi_selftest_snp.c
+++ b/lib/efi_selftest/efi_selftest_snp.c
@@ -260,7 +260,7 @@ static int execute(void)
 {
 	efi_status_t ret;
 	struct efi_event *events[2];
-	size_t index;
+	efi_uintn_t index;
 	union {
 		struct dhcp p;
 		u8 b[PKTSIZE];
diff --git a/lib/efi_selftest/efi_selftest_textoutput.c b/lib/efi_selftest/efi_selftest_textoutput.c
new file mode 100644
index 0000000..6e8c90c
--- /dev/null
+++ b/lib/efi_selftest/efi_selftest_textoutput.c
@@ -0,0 +1,53 @@
+/*
+ * efi_selftest_textoutput
+ *
+ * Copyright (c) 2017 Heinrich Schuchardt <xypron.glpk@gmx.de>
+ *
+ * SPDX-License-Identifier:     GPL-2.0+
+ *
+ * Test the EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL.
+ *
+ * The following services are tested:
+ * OutputString, TestString, SetAttribute.
+ */
+
+#include <efi_selftest.h>
+
+/*
+ * Execute unit test.
+ *
+ * @return:	EFI_ST_SUCCESS for success
+ */
+static int execute(void)
+{
+	size_t foreground;
+	size_t background;
+	size_t attrib;
+	efi_status_t ret;
+
+	/* SetAttribute */
+	efi_st_printf("\nColor palette\n");
+	for (foreground = 0; foreground < 0x10; ++foreground) {
+		for (background = 0; background < 0x80; background += 0x10) {
+			attrib = foreground | background;
+			con_out->set_attribute(con_out, attrib);
+			efi_st_printf("%p", (void *)attrib);
+		}
+		con_out->set_attribute(con_out, 0);
+		efi_st_printf("\n");
+	}
+	/* TestString */
+	ret = con_out->test_string(con_out,
+			L" !\"#$%&'()*+,-./0-9:;<=>?@A-Z[\\]^_`a-z{|}~\n");
+	if (ret != EFI_ST_SUCCESS) {
+		efi_st_error("TestString failed for ANSI characters\n");
+		return EFI_ST_FAILURE;
+	}
+	return EFI_ST_SUCCESS;
+}
+
+EFI_UNIT_TEST(textoutput) = {
+	.name = "text output",
+	.phase = EFI_EXECUTE_BEFORE_BOOTTIME_EXIT,
+	.execute = execute,
+};
diff --git a/lib/efi_selftest/efi_selftest_tpl.c b/lib/efi_selftest/efi_selftest_tpl.c
index ddb67ed..6ea0bb7 100644
--- a/lib/efi_selftest/efi_selftest_tpl.c
+++ b/lib/efi_selftest/efi_selftest_tpl.c
@@ -111,9 +111,9 @@ static int teardown(void)
  */
 static int execute(void)
 {
-	size_t index;
+	efi_uintn_t index;
 	efi_status_t ret;
-	UINTN old_tpl;
+	efi_uintn_t old_tpl;
 
 	/* Set 10 ms timer */
 	notification_count = 0;
diff --git a/lib/efi_selftest/efi_selftest_util.c b/lib/efi_selftest/efi_selftest_util.c
index 5cffe38..1b17bf4 100644
--- a/lib/efi_selftest/efi_selftest_util.c
+++ b/lib/efi_selftest/efi_selftest_util.c
@@ -21,5 +21,14 @@ int efi_st_memcmp(const void *buf1, const void *buf2, size_t length)
 		++pos1;
 		++pos2;
 	}
-	return EFI_ST_SUCCESS;
+	return 0;
+}
+
+int efi_st_strcmp_16_8(const u16 *buf1, const char *buf2)
+{
+	for (; *buf1 || *buf2; ++buf1, ++buf2) {
+		if (*buf1 != *buf2)
+			return *buf1 - *buf2;
+	}
+	return 0;
 }
diff --git a/lib/efi_selftest/efi_selftest_watchdog.c b/lib/efi_selftest/efi_selftest_watchdog.c
new file mode 100644
index 0000000..e4af384
--- /dev/null
+++ b/lib/efi_selftest/efi_selftest_watchdog.c
@@ -0,0 +1,231 @@
+/*
+ * efi_selftest_watchdog
+ *
+ * Copyright (c) 2017 Heinrich Schuchardt <xypron.glpk@gmx.de>
+ *
+ * SPDX-License-Identifier:     GPL-2.0+
+ *
+ * The 'watchdog timer' unit test checks that the watchdog timer
+ * will not cause a system restart during the timeout period after
+ * a timer reset.
+ *
+ * The 'watchdog reboot' unit test checks that the watchdog timer
+ * actually reboots the system after a timeout. The test is only
+ * executed on explicit request. Use the following commands:
+ *
+ *	setenv efi_selftest watchdog reboot
+ *	bootefi selftest
+ */
+
+#include <efi_selftest.h>
+
+/*
+ * This is the communication structure for the notification function.
+ */
+struct notify_context {
+	/* Status code returned when resetting watchdog */
+	efi_status_t status;
+	/* Number of invocations of the notification function */
+	unsigned int timer_ticks;
+};
+
+static struct efi_event *event_notify;
+static struct efi_event *event_wait;
+static struct efi_boot_services *boottime;
+static struct notify_context notification_context;
+static bool watchdog_reset;
+
+/*
+ * Notification function, increments the notfication count if parameter
+ * context is provided.
+ *
+ * @event	notified event
+ * @context	pointer to the timeout
+ */
+static void EFIAPI notify(struct efi_event *event, void *context)
+{
+	struct notify_context *notify_context = context;
+	efi_status_t ret = EFI_SUCCESS;
+
+	if (!notify_context)
+		return;
+
+	/* Reset watchdog timer to one second */
+	ret = boottime->set_watchdog_timer(1, 0, 0, NULL);
+	if (ret != EFI_SUCCESS)
+		notify_context->status = ret;
+	/* Count number of calls */
+	notify_context->timer_ticks++;
+}
+
+/*
+ * Setup unit test.
+ *
+ * Create two timer events.
+ * One with EVT_NOTIFY_SIGNAL, the other with EVT_NOTIFY_WAIT.
+ *
+ * @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;
+
+	boottime = systable->boottime;
+
+	notification_context.status = EFI_SUCCESS;
+	notification_context.timer_ticks = 0;
+	ret = boottime->create_event(EVT_TIMER | EVT_NOTIFY_SIGNAL,
+				     TPL_CALLBACK, notify,
+				     (void *)&notification_context,
+				     &event_notify);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("could not create event\n");
+		return EFI_ST_FAILURE;
+	}
+	ret = boottime->create_event(EVT_TIMER | EVT_NOTIFY_WAIT,
+				     TPL_CALLBACK, notify, NULL, &event_wait);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("could not create event\n");
+		return EFI_ST_FAILURE;
+	}
+	return EFI_ST_SUCCESS;
+}
+
+/*
+ * Execute the test resetting the watchdog in a timely manner. No reboot occurs.
+ *
+ * @handle:	handle of the loaded image
+ * @systable:	system table
+ * @return:	EFI_ST_SUCCESS for success
+ */
+static int setup_timer(const efi_handle_t handle,
+		       const struct efi_system_table *systable)
+{
+	watchdog_reset = true;
+	return setup(handle, systable);
+}
+
+/*
+ * Execute the test without resetting the watchdog. A system reboot occurs.
+ *
+ * @handle:	handle of the loaded image
+ * @systable:	system table
+ * @return:	EFI_ST_SUCCESS for success
+ */
+static int setup_reboot(const efi_handle_t handle,
+			const struct efi_system_table *systable)
+{
+	watchdog_reset = false;
+	return setup(handle, systable);
+}
+
+/*
+ * Tear down unit test.
+ *
+ * Close the events created in setup.
+ *
+ * @return:	EFI_ST_SUCCESS for success
+ */
+static int teardown(void)
+{
+	efi_status_t ret;
+
+	/* Set the watchdog timer to the five minute default value */
+	ret = boottime->set_watchdog_timer(300, 0, 0, NULL);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Setting watchdog timer failed\n");
+		return EFI_ST_FAILURE;
+	}
+	if (event_notify) {
+		ret = boottime->close_event(event_notify);
+		event_notify = NULL;
+		if (ret != EFI_SUCCESS) {
+			efi_st_error("Could not close event\n");
+			return EFI_ST_FAILURE;
+		}
+	}
+	if (event_wait) {
+		ret = boottime->close_event(event_wait);
+		event_wait = NULL;
+		if (ret != EFI_SUCCESS) {
+			efi_st_error("Could not close event\n");
+			return EFI_ST_FAILURE;
+		}
+	}
+	return EFI_ST_SUCCESS;
+}
+
+/*
+ * Execute unit test.
+ *
+ * Run a 600 ms periodic timer that resets the watchdog to one second
+ * on every timer tick.
+ *
+ * Run a 1350 ms single shot timer and check that the 600ms timer has
+ * been called 2 times.
+ *
+ * @return:	EFI_ST_SUCCESS for success
+ */
+static int execute(void)
+{
+	size_t index;
+	efi_status_t ret;
+
+	/* Set the watchdog timeout to one second */
+	ret = boottime->set_watchdog_timer(1, 0, 0, NULL);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Setting watchdog timer failed\n");
+		return EFI_ST_FAILURE;
+	}
+	if (watchdog_reset) {
+		/* Set 600 ms timer */
+		ret = boottime->set_timer(event_notify, EFI_TIMER_PERIODIC,
+					  6000000);
+		if (ret != EFI_SUCCESS) {
+			efi_st_error("Could not set timer\n");
+			return EFI_ST_FAILURE;
+		}
+	}
+	/* Set 1350 ms timer */
+	ret = boottime->set_timer(event_wait, EFI_TIMER_RELATIVE, 13500000);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Could not set timer\n");
+		return EFI_ST_FAILURE;
+	}
+
+	ret = boottime->wait_for_event(1, &event_wait, &index);
+	if (ret != EFI_SUCCESS) {
+		efi_st_error("Could not wait for event\n");
+		return EFI_ST_FAILURE;
+	}
+	if (notification_context.status != EFI_SUCCESS) {
+		efi_st_error("Setting watchdog timer failed\n");
+		return EFI_ST_FAILURE;
+	}
+	if (notification_context.timer_ticks != 2) {
+		efi_st_error("The timer was called %u times, expected 2.\n",
+			     notification_context.timer_ticks);
+		return EFI_ST_FAILURE;
+	}
+	return EFI_ST_SUCCESS;
+}
+
+EFI_UNIT_TEST(watchdog1) = {
+	.name = "watchdog timer",
+	.phase = EFI_EXECUTE_BEFORE_BOOTTIME_EXIT,
+	.setup = setup_timer,
+	.execute = execute,
+	.teardown = teardown,
+};
+
+EFI_UNIT_TEST(watchdog2) = {
+	.name = "watchdog reboot",
+	.phase = EFI_EXECUTE_BEFORE_BOOTTIME_EXIT,
+	.setup = setup_reboot,
+	.execute = execute,
+	.teardown = teardown,
+	.on_request = true,
+};