X-Git-Url: https://git.sur5r.net/?a=blobdiff_plain;f=lib%2Fefi_loader%2Fefi_boottime.c;h=ff11d028da8a2e8068ce1e90f2d291f1bbf18e32;hb=476ed96e01bf4b103a4bf09f1bbcb8335eb0eb07;hp=0290d213f2027673062c1ca7bd963f5026513648;hpb=cd534083cd230e6a38ac575b62a3fb3b93521e9a;p=u-boot

diff --git a/lib/efi_loader/efi_boottime.c b/lib/efi_loader/efi_boottime.c
index 0290d213f2..ff11d028da 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);
@@ -56,6 +56,14 @@ static volatile void *efi_gd, *app_gd;
 
 static int entry_count;
 static int nesting_level;
+/* GUID of the EFI_DRIVER_BINDING_PROTOCOL */
+const efi_guid_t efi_guid_driver_binding_protocol =
+			EFI_DRIVER_BINDING_PROTOCOL_GUID;
+
+static efi_status_t EFIAPI efi_disconnect_controller(
+					efi_handle_t controller_handle,
+					efi_handle_t driver_image_handle,
+					efi_handle_t child_handle);
 
 /* Called on every callback entry */
 int __efi_entry_check(void)
@@ -165,9 +173,9 @@ void efi_signal_event(struct efi_event *event)
  * @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);
 
@@ -190,7 +198,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);
 
@@ -217,12 +225,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);
 }
@@ -239,11 +247,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);
 }
@@ -264,10 +272,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;
@@ -292,12 +300,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);
 }
@@ -321,7 +329,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(efi_handle_t *handle)
 {
 	struct efi_object *obj;
 	efi_status_t r;
@@ -331,13 +362,109 @@ 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;
 }
 
+/*
+ * Find a protocol on a handle.
+ *
+ * @handle		handle
+ * @protocol_guid	GUID of the protocol
+ * @handler		reference to the protocol
+ * @return		status code
+ */
+efi_status_t efi_search_protocol(const efi_handle_t 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;
+}
+
+/*
+ * 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 efi_handle_t handle,
+				 const efi_guid_t *protocol,
+				 void *protocol_interface)
+{
+	struct efi_handler *handler;
+	efi_status_t ret;
+
+	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;
+}
+
+/*
+ * 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 efi_handle_t handle)
+{
+	struct efi_object *efiobj;
+	struct efi_handler *protocol;
+	struct efi_handler *pos;
+
+	efiobj = efi_search_obj(handle);
+	if (!efiobj)
+		return EFI_INVALID_PARAMETER;
+	list_for_each_entry_safe(protocol, pos, &efiobj->protocols, link) {
+		efi_status_t ret;
+
+		ret = efi_remove_protocol(handle, protocol->guid,
+					  protocol->protocol_interface);
+		if (ret != EFI_SUCCESS)
+			return ret;
+	}
+	return EFI_SUCCESS;
+}
+
+/*
+ * Delete handle.
+ *
+ * @handle	handle to delete
+ */
+void efi_delete_handle(struct efi_object *obj)
+{
+	if (!obj)
+		return;
+	efi_remove_all_protocols(obj->handle);
+	list_del(&obj->link);
+	free(obj);
+}
+
 /*
  * Our event capabilities are very limited. Only a small limited
  * number of events is allowed to coexist.
@@ -359,7 +486,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),
@@ -409,7 +536,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),
@@ -539,13 +666,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)
@@ -684,14 +811,11 @@ static efi_status_t EFIAPI efi_check_event(struct efi_event *event)
  * @handle	handle to find
  * @return	EFI object
  */
-static struct efi_object *efi_search_obj(void *handle)
+struct efi_object *efi_search_obj(const efi_handle_t handle)
 {
-	struct list_head *lhandle;
-
-	list_for_each(lhandle, &efi_obj_list) {
-		struct efi_object *efiobj;
+	struct efi_object *efiobj;
 
-		efiobj = list_entry(lhandle, struct efi_object, link);
+	list_for_each_entry(efiobj, &efi_obj_list, link) {
 		if (efiobj->handle == handle)
 			return efiobj;
 	}
@@ -699,6 +823,74 @@ static struct efi_object *efi_search_obj(void *handle)
 	return NULL;
 }
 
+/*
+ * Create open protocol info entry and add it to a protocol.
+ *
+ * @handler	handler of a protocol
+ * @return	open protocol info entry
+ */
+static struct efi_open_protocol_info_entry *efi_create_open_info(
+			struct efi_handler *handler)
+{
+	struct efi_open_protocol_info_item *item;
+
+	item = calloc(1, sizeof(struct efi_open_protocol_info_item));
+	if (!item)
+		return NULL;
+	/* Append the item to the open protocol info list. */
+	list_add_tail(&item->link, &handler->open_infos);
+
+	return &item->info;
+}
+
+/*
+ * Remove an open protocol info entry from a protocol.
+ *
+ * @handler	handler of a protocol
+ * @return	status code
+ */
+static efi_status_t efi_delete_open_info(
+			struct efi_open_protocol_info_item *item)
+{
+	list_del(&item->link);
+	free(item);
+	return EFI_SUCCESS;
+}
+
+/*
+ * Install new protocol on a handle.
+ *
+ * @handle			handle on which the protocol shall be installed
+ * @protocol			GUID of the protocol to be installed
+ * @protocol_interface		interface of the protocol implementation
+ * @return			status code
+ */
+efi_status_t efi_add_protocol(const efi_handle_t handle,
+			      const efi_guid_t *protocol,
+			      void *protocol_interface)
+{
+	struct efi_object *efiobj;
+	struct efi_handler *handler;
+	efi_status_t ret;
+
+	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;
+	INIT_LIST_HEAD(&handler->open_infos);
+	list_add_tail(&handler->link, &efiobj->protocols);
+	if (!guidcmp(&efi_guid_device_path, protocol))
+		EFI_PRINT("installed device path '%pD'\n", protocol_interface);
+	return EFI_SUCCESS;
+}
+
 /*
  * Install protocol interface.
  *
@@ -717,8 +909,6 @@ static efi_status_t EFIAPI efi_install_protocol_interface(
 			void **handle, const efi_guid_t *protocol,
 			int protocol_interface_type, void *protocol_interface)
 {
-	struct list_head *lhandle;
-	int i;
 	efi_status_t r;
 
 	EFI_ENTRY("%p, %pUl, %d, %p", handle, protocol, protocol_interface_type,
@@ -735,41 +925,14 @@ static efi_status_t EFIAPI efi_install_protocol_interface(
 		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);
 	}
-	/* Find object. */
-	list_for_each(lhandle, &efi_obj_list) {
-		struct efi_object *efiobj;
-		efiobj = list_entry(lhandle, struct efi_object, link);
-
-		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];
-
-			if (handler->guid)
-				continue;
-
-			handler->guid = protocol;
-			handler->protocol_interface = protocol_interface;
-			r = EFI_SUCCESS;
-			goto out;
-		}
-		r = EFI_OUT_OF_RESOURCES;
-		goto out;
-	}
-	r = EFI_INVALID_PARAMETER;
+	/* Add new protocol */
+	r = efi_add_protocol(*handle, protocol, protocol_interface);
 out:
 	return EFI_EXIT(r);
 }
@@ -788,15 +951,118 @@ out:
  * @new_interface		interface to be installed
  * @return			status code
  */
-static efi_status_t EFIAPI efi_reinstall_protocol_interface(void *handle,
-			const efi_guid_t *protocol, void *old_interface,
-			void *new_interface)
+static efi_status_t EFIAPI efi_reinstall_protocol_interface(
+			efi_handle_t handle, const efi_guid_t *protocol,
+			void *old_interface, void *new_interface)
 {
 	EFI_ENTRY("%p, %pUl, %p, %p", handle, protocol, old_interface,
 		  new_interface);
 	return EFI_EXIT(EFI_ACCESS_DENIED);
 }
 
+/*
+ * Get all drivers associated to a controller.
+ * The allocated buffer has to be freed with free().
+ *
+ * @efiobj			handle of the controller
+ * @protocol			protocol guid (optional)
+ * @number_of_drivers		number of child controllers
+ * @driver_handle_buffer	handles of the the drivers
+ * @return			status code
+ */
+static efi_status_t efi_get_drivers(struct efi_object *efiobj,
+				    const efi_guid_t *protocol,
+				    efi_uintn_t *number_of_drivers,
+				    efi_handle_t **driver_handle_buffer)
+{
+	struct efi_handler *handler;
+	struct efi_open_protocol_info_item *item;
+	efi_uintn_t count = 0, i;
+	bool duplicate;
+
+	/* Count all driver associations */
+	list_for_each_entry(handler, &efiobj->protocols, link) {
+		if (protocol && guidcmp(handler->guid, protocol))
+			continue;
+		list_for_each_entry(item, &handler->open_infos, link) {
+			if (item->info.attributes &
+			    EFI_OPEN_PROTOCOL_BY_DRIVER)
+				++count;
+		}
+	}
+	/*
+	 * Create buffer. In case of duplicate driver assignments the buffer
+	 * will be too large. But that does not harm.
+	 */
+	*number_of_drivers = 0;
+	*driver_handle_buffer = calloc(count, sizeof(efi_handle_t));
+	if (!*driver_handle_buffer)
+		return EFI_OUT_OF_RESOURCES;
+	/* Collect unique driver handles */
+	list_for_each_entry(handler, &efiobj->protocols, link) {
+		if (protocol && guidcmp(handler->guid, protocol))
+			continue;
+		list_for_each_entry(item, &handler->open_infos, link) {
+			if (item->info.attributes &
+			    EFI_OPEN_PROTOCOL_BY_DRIVER) {
+				/* Check this is a new driver */
+				duplicate = false;
+				for (i = 0; i < *number_of_drivers; ++i) {
+					if ((*driver_handle_buffer)[i] ==
+					    item->info.agent_handle)
+						duplicate = true;
+				}
+				/* Copy handle to buffer */
+				if (!duplicate) {
+					i = (*number_of_drivers)++;
+					(*driver_handle_buffer)[i] =
+						item->info.agent_handle;
+				}
+			}
+		}
+	}
+	return EFI_SUCCESS;
+}
+
+/*
+ * Disconnect all drivers from a controller.
+ *
+ * This function implements the DisconnectController service.
+ * See the Unified Extensible Firmware Interface (UEFI) specification
+ * for details.
+ *
+ * @efiobj		handle of the controller
+ * @protocol		protocol guid (optional)
+ * @child_handle	handle of the child to destroy
+ * @return		status code
+ */
+static efi_status_t efi_disconnect_all_drivers(
+				struct efi_object *efiobj,
+				const efi_guid_t *protocol,
+				efi_handle_t child_handle)
+{
+	efi_uintn_t number_of_drivers;
+	efi_handle_t *driver_handle_buffer;
+	efi_status_t r, ret;
+
+	ret = efi_get_drivers(efiobj, protocol, &number_of_drivers,
+			      &driver_handle_buffer);
+	if (ret != EFI_SUCCESS)
+		return ret;
+
+	ret = EFI_NOT_FOUND;
+	while (number_of_drivers) {
+		r = EFI_CALL(efi_disconnect_controller(
+				efiobj->handle,
+				driver_handle_buffer[--number_of_drivers],
+				child_handle));
+		if (r == EFI_SUCCESS)
+			ret = r;
+	}
+	free(driver_handle_buffer);
+	return ret;
+}
+
 /*
  * Uninstall protocol interface.
  *
@@ -810,45 +1076,46 @@ static efi_status_t EFIAPI efi_reinstall_protocol_interface(void *handle,
  * @return			status code
  */
 static efi_status_t EFIAPI efi_uninstall_protocol_interface(
-				void *handle, const efi_guid_t *protocol,
+				efi_handle_t handle, const efi_guid_t *protocol,
 				void *protocol_interface)
 {
-	struct list_head *lhandle;
-	int i;
-	efi_status_t r = EFI_NOT_FOUND;
+	struct efi_object *efiobj;
+	struct efi_handler *handler;
+	struct efi_open_protocol_info_item *item;
+	struct efi_open_protocol_info_item *pos;
+	efi_status_t r;
 
 	EFI_ENTRY("%p, %pUl, %p", handle, protocol, protocol_interface);
 
-	if (!handle || !protocol) {
+	/* Check handle */
+	efiobj = efi_search_obj(handle);
+	if (!efiobj) {
 		r = EFI_INVALID_PARAMETER;
 		goto out;
 	}
-
-	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;
+	/* Disconnect controllers */
+	efi_disconnect_all_drivers(efiobj, protocol, NULL);
+	if (!list_empty(&handler->open_infos)) {
+		r =  EFI_ACCESS_DENIED;
+		goto out;
 	}
-
+	/* Close protocol */
+	list_for_each_entry_safe(item, pos, &handler->open_infos, link) {
+		if (item->info.attributes ==
+			EFI_OPEN_PROTOCOL_BY_HANDLE_PROTOCOL ||
+		    item->info.attributes == EFI_OPEN_PROTOCOL_GET_PROTOCOL ||
+		    item->info.attributes == EFI_OPEN_PROTOCOL_TEST_PROTOCOL)
+			list_del(&item->link);
+	}
+	if (!list_empty(&handler->open_infos)) {
+		r =  EFI_ACCESS_DENIED;
+		goto out;
+	}
+	r = efi_remove_protocol(handle, protocol, protocol_interface);
 out:
 	return EFI_EXIT(r);
 }
@@ -890,23 +1157,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:
 		return 0;
 	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;
-		}
+		ret = efi_search_protocol(efiobj->handle, protocol, NULL);
+		return (ret != EFI_SUCCESS);
+	default:
+		/* Invalid search type */
 		return -1;
 	}
-
-	return -1;
 }
 
 /*
@@ -925,18 +1190,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) {
@@ -949,12 +1236,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;
@@ -977,7 +1261,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);
@@ -986,36 +1270,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)
 {
@@ -1094,39 +1348,55 @@ static efi_status_t EFIAPI efi_install_configuration_table_ext(efi_guid_t *guid,
  * @obj			internal object associated with the loaded image
  * @device_path		device path of the loaded image
  * @file_path		file path of the loaded image
+ * @return		status code
  */
-void efi_setup_loaded_image(struct efi_loaded_image *info, struct efi_object *obj,
-			    struct efi_device_path *device_path,
-			    struct efi_device_path *file_path)
+efi_status_t efi_setup_loaded_image(
+			struct efi_loaded_image *info, struct efi_object *obj,
+			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 ret;
+failure:
+	printf("ERROR: Failure to install protocols for loaded image\n");
+	return ret;
 }
 
 /*
@@ -1202,8 +1472,9 @@ static efi_status_t EFIAPI efi_load_image(bool boot_policy,
 {
 	struct efi_loaded_image *info;
 	struct efi_object *obj;
+	efi_status_t ret;
 
-	EFI_ENTRY("%d, %p, %p, %p, %ld, %p", boot_policy, parent_image,
+	EFI_ENTRY("%d, %p, %pD, %p, %ld, %p", boot_policy, parent_image,
 		  file_path, source_buffer, source_size, image_handle);
 
 	info = calloc(1, sizeof(*info));
@@ -1211,41 +1482,39 @@ static efi_status_t EFIAPI efi_load_image(bool boot_policy,
 
 	if (!source_buffer) {
 		struct efi_device_path *dp, *fp;
-		efi_status_t ret;
 
 		ret = efi_load_image_from_path(file_path, &source_buffer);
-		if (ret != EFI_SUCCESS) {
-			free(info);
-			free(obj);
-			return EFI_EXIT(ret);
-		}
-
+		if (ret != EFI_SUCCESS)
+			goto failure;
 		/*
 		 * split file_path which contains both the device and
 		 * file parts:
 		 */
 		efi_dp_split_file_path(file_path, &dp, &fp);
-
-		efi_setup_loaded_image(info, obj, dp, fp);
+		ret = efi_setup_loaded_image(info, obj, dp, fp);
+		if (ret != EFI_SUCCESS)
+			goto failure;
 	} else {
 		/* In this case, file_path is the "device" path, ie.
 		 * something like a HARDWARE_DEVICE:MEMORY_MAPPED
 		 */
-		efi_setup_loaded_image(info, obj, file_path, NULL);
+		ret = efi_setup_loaded_image(info, obj, file_path, NULL);
+		if (ret != EFI_SUCCESS)
+			goto failure;
 	}
-
 	info->reserved = efi_load_pe(source_buffer, info);
 	if (!info->reserved) {
-		free(info);
-		free(obj);
-		return EFI_EXIT(EFI_UNSUPPORTED);
+		ret = EFI_UNSUPPORTED;
+		goto failure;
 	}
-
 	info->system_table = &systab;
 	info->parent_handle = parent_image;
-	*image_handle = info;
-
+	*image_handle = obj->handle;
 	return EFI_EXIT(EFI_SUCCESS);
+failure:
+	free(info);
+	efi_delete_handle(obj);
+	return EFI_EXIT(ret);
 }
 
 /*
@@ -1264,8 +1533,9 @@ static efi_status_t EFIAPI efi_start_image(efi_handle_t image_handle,
 					   unsigned long *exit_data_size,
 					   s16 **exit_data)
 {
-	ulong (*entry)(void *image_handle, struct efi_system_table *st);
+	ulong (*entry)(efi_handle_t image_handle, struct efi_system_table *st);
 	struct efi_loaded_image *info = image_handle;
+	efi_status_t ret;
 
 	EFI_ENTRY("%p, %p, %p", image_handle, exit_data_size, exit_data);
 	entry = info->reserved;
@@ -1274,18 +1544,37 @@ static efi_status_t EFIAPI efi_start_image(efi_handle_t image_handle,
 
 	/* call the image! */
 	if (setjmp(&info->exit_jmp)) {
-		/* We returned from the child image */
+		/*
+		 * We called the entry point of the child image with EFI_CALL
+		 * in the lines below. The child image called the Exit() boot
+		 * service efi_exit() which executed the long jump that brought
+		 * us to the current line. This implies that the second half
+		 * of the EFI_CALL macro has not been executed.
+		 */
+#ifdef CONFIG_ARM
+		/*
+		 * efi_exit() called efi_restore_gd(). We have to undo this
+		 * otherwise __efi_entry_check() will put the wrong value into
+		 * app_gd.
+		 */
+		gd = app_gd;
+#endif
+		/*
+		 * To get ready to call EFI_EXIT below we have to execute the
+		 * missed out steps of EFI_CALL.
+		 */
+		assert(__efi_entry_check());
+		debug("%sEFI: %lu returned by started image\n",
+		      __efi_nesting_dec(),
+		      (unsigned long)((uintptr_t)info->exit_status &
+				      ~EFI_ERROR_MASK));
 		return EFI_EXIT(info->exit_status);
 	}
 
-	__efi_nesting_dec();
-	__efi_exit_check();
-	entry(image_handle, &systab);
-	__efi_entry_check();
-	__efi_nesting_inc();
+	ret = EFI_CALL(entry(image_handle, &systab));
 
 	/* Should usually never get here */
-	return EFI_EXIT(EFI_SUCCESS);
+	return EFI_EXIT(ret);
 }
 
 /*
@@ -1305,13 +1594,24 @@ 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,
 		  exit_data_size, exit_data);
 
 	/* Make sure entry/exit counts for EFI world cross-overs match */
-	__efi_exit_check();
+	EFI_EXIT(exit_status);
 
 	/*
 	 * But longjmp out with the U-Boot gd, not the application's, as
@@ -1335,7 +1635,7 @@ static efi_status_t EFIAPI efi_exit(efi_handle_t image_handle,
  * @image_handle	handle of the image to be unloaded
  * @return		status code
  */
-static efi_status_t EFIAPI efi_unload_image(void *image_handle)
+static efi_status_t EFIAPI efi_unload_image(efi_handle_t image_handle)
 {
 	struct efi_object *efiobj;
 
@@ -1373,7 +1673,7 @@ static void efi_exit_caches(void)
  * @map_key		key of the memory map
  * @return		status code
  */
-static efi_status_t EFIAPI efi_exit_boot_services(void *image_handle,
+static efi_status_t EFIAPI efi_exit_boot_services(efi_handle_t image_handle,
 						  unsigned long map_key)
 {
 	int i;
@@ -1464,51 +1764,6 @@ static efi_status_t EFIAPI efi_set_watchdog_timer(unsigned long timeout,
 	return EFI_EXIT(efi_set_watchdog(timeout));
 }
 
-/*
- * Connect a controller to a driver.
- *
- * This function implements the ConnectController service.
- * See the Unified Extensible Firmware Interface (UEFI) specification
- * for details.
- *
- * @controller_handle	handle of the controller
- * @driver_image_handle	handle of the driver
- * @remain_device_path	device path of a child controller
- * @recursive		true to connect all child controllers
- * @return		status code
- */
-static efi_status_t EFIAPI efi_connect_controller(
-			efi_handle_t controller_handle,
-			efi_handle_t *driver_image_handle,
-			struct efi_device_path *remain_device_path,
-			bool recursive)
-{
-	EFI_ENTRY("%p, %p, %p, %d", controller_handle, driver_image_handle,
-		  remain_device_path, recursive);
-	return EFI_EXIT(EFI_NOT_FOUND);
-}
-
-/*
- * Disconnect a controller from a driver.
- *
- * This function implements the DisconnectController service.
- * See the Unified Extensible Firmware Interface (UEFI) specification
- * for details.
- *
- * @controller_handle	handle of the controller
- * @driver_image_handle handle of the driver
- * @child_handle	handle of the child to destroy
- * @return		status code
- */
-static efi_status_t EFIAPI efi_disconnect_controller(void *controller_handle,
-						     void *driver_image_handle,
-						     void *child_handle)
-{
-	EFI_ENTRY("%p, %p, %p", controller_handle, driver_image_handle,
-		  child_handle);
-	return EFI_EXIT(EFI_INVALID_PARAMETER);
-}
-
 /*
  * Close a protocol.
  *
@@ -1522,14 +1777,38 @@ static efi_status_t EFIAPI efi_disconnect_controller(void *controller_handle,
  * @controller_handle	handle of the controller
  * @return		status code
  */
-static efi_status_t EFIAPI efi_close_protocol(void *handle,
+static efi_status_t EFIAPI efi_close_protocol(efi_handle_t handle,
 					      const efi_guid_t *protocol,
-					      void *agent_handle,
-					      void *controller_handle)
+					      efi_handle_t agent_handle,
+					      efi_handle_t controller_handle)
 {
+	struct efi_handler *handler;
+	struct efi_open_protocol_info_item *item;
+	struct efi_open_protocol_info_item *pos;
+	efi_status_t r;
+
 	EFI_ENTRY("%p, %pUl, %p, %p", handle, protocol, agent_handle,
 		  controller_handle);
-	return EFI_EXIT(EFI_NOT_FOUND);
+
+	if (!agent_handle) {
+		r = EFI_INVALID_PARAMETER;
+		goto out;
+	}
+	r = efi_search_protocol(handle, protocol, &handler);
+	if (r != EFI_SUCCESS)
+		goto out;
+
+	r = EFI_NOT_FOUND;
+	list_for_each_entry_safe(item, pos, &handler->open_infos, link) {
+		if (item->info.agent_handle == agent_handle &&
+		    item->info.controller_handle == controller_handle) {
+			efi_delete_open_info(item);
+			r = EFI_SUCCESS;
+			break;
+		}
+	}
+out:
+	return EFI_EXIT(r);
 }
 
 /*
@@ -1548,11 +1827,51 @@ 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)
 {
+	unsigned long buffer_size;
+	unsigned long count;
+	struct efi_handler *handler;
+	struct efi_open_protocol_info_item *item;
+	efi_status_t r;
+
 	EFI_ENTRY("%p, %pUl, %p, %p", handle, protocol, entry_buffer,
 		  entry_count);
-	return EFI_EXIT(EFI_NOT_FOUND);
+
+	/* Check parameters */
+	if (!entry_buffer) {
+		r = EFI_INVALID_PARAMETER;
+		goto out;
+	}
+	r = efi_search_protocol(handle, protocol, &handler);
+	if (r != EFI_SUCCESS)
+		goto out;
+
+	/* Count entries */
+	count = 0;
+	list_for_each_entry(item, &handler->open_infos, link) {
+		if (item->info.open_count)
+			++count;
+	}
+	*entry_count = count;
+	*entry_buffer = NULL;
+	if (!count) {
+		r = EFI_SUCCESS;
+		goto out;
+	}
+
+	/* Copy entries */
+	buffer_size = count * sizeof(struct efi_open_protocol_info_entry);
+	r = efi_allocate_pool(EFI_ALLOCATE_ANY_PAGES, buffer_size,
+			      (void **)entry_buffer);
+	if (r != EFI_SUCCESS)
+		goto out;
+	list_for_each_entry_reverse(item, &handler->open_infos, link) {
+		if (item->info.open_count)
+			(*entry_buffer)[--count] = item->info;
+	}
+out:
+	return EFI_EXIT(r);
 }
 
 /*
@@ -1567,14 +1886,13 @@ static efi_status_t EFIAPI efi_open_protocol_information(efi_handle_t handle,
  * @protocol_buffer_count	number of entries in the buffer
  * @return			status code
  */
-static efi_status_t EFIAPI efi_protocols_per_handle(void *handle,
-			efi_guid_t ***protocol_buffer,
-			unsigned long *protocol_buffer_count)
+static efi_status_t EFIAPI efi_protocols_per_handle(
+			efi_handle_t handle, efi_guid_t ***protocol_buffer,
+			efi_uintn_t *protocol_buffer_count)
 {
 	unsigned long buffer_size;
 	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,
@@ -1585,36 +1903,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);
@@ -1637,10 +1952,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);
@@ -1662,7 +1977,7 @@ static efi_status_t EFIAPI efi_locate_handle_buffer(
 	r = efi_locate_handle(search_type, protocol, search_key, &buffer_size,
 			      *buffer);
 	if (r == EFI_SUCCESS)
-		*no_handles = buffer_size / sizeof(void *);
+		*no_handles = buffer_size / sizeof(efi_handle_t);
 out:
 	return EFI_EXIT(r);
 }
@@ -1684,29 +1999,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;
@@ -1714,6 +2023,82 @@ static efi_status_t EFIAPI efi_locate_protocol(const efi_guid_t *protocol,
 	return EFI_EXIT(EFI_NOT_FOUND);
 }
 
+/*
+ * 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.
  *
@@ -1758,7 +2143,7 @@ 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*);
@@ -1787,7 +2172,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);
 }
 
 /*
@@ -1848,6 +2271,101 @@ static void EFIAPI efi_set_mem(void *buffer, size_t size, uint8_t value)
 	EFI_EXIT(EFI_SUCCESS);
 }
 
+/*
+ * Open protocol interface on a handle.
+ *
+ * @handler		handler of a protocol
+ * @protocol_interface	interface implementing the protocol
+ * @agent_handle	handle of the driver
+ * @controller_handle	handle of the controller
+ * @attributes		attributes indicating how to open the protocol
+ * @return		status code
+ */
+static efi_status_t efi_protocol_open(
+			struct efi_handler *handler,
+			void **protocol_interface, void *agent_handle,
+			void *controller_handle, uint32_t attributes)
+{
+	struct efi_open_protocol_info_item *item;
+	struct efi_open_protocol_info_entry *match = NULL;
+	bool opened_by_driver = false;
+	bool opened_exclusive = false;
+
+	/* If there is no agent, only return the interface */
+	if (!agent_handle)
+		goto out;
+
+	/* For TEST_PROTOCOL ignore interface attribute */
+	if (attributes != EFI_OPEN_PROTOCOL_TEST_PROTOCOL)
+		*protocol_interface = NULL;
+
+	/*
+	 * Check if the protocol is already opened by a driver with the same
+	 * attributes or opened exclusively
+	 */
+	list_for_each_entry(item, &handler->open_infos, link) {
+		if (item->info.agent_handle == agent_handle) {
+			if ((attributes & EFI_OPEN_PROTOCOL_BY_DRIVER) &&
+			    (item->info.attributes == attributes))
+				return EFI_ALREADY_STARTED;
+		}
+		if (item->info.attributes & EFI_OPEN_PROTOCOL_EXCLUSIVE)
+			opened_exclusive = true;
+	}
+
+	/* Only one controller can open the protocol exclusively */
+	if (opened_exclusive && attributes &
+	    (EFI_OPEN_PROTOCOL_EXCLUSIVE | EFI_OPEN_PROTOCOL_BY_DRIVER))
+		return EFI_ACCESS_DENIED;
+
+	/* Prepare exclusive opening */
+	if (attributes & EFI_OPEN_PROTOCOL_EXCLUSIVE) {
+		/* Try to disconnect controllers */
+		list_for_each_entry(item, &handler->open_infos, link) {
+			if (item->info.attributes ==
+					EFI_OPEN_PROTOCOL_BY_DRIVER)
+				EFI_CALL(efi_disconnect_controller(
+						item->info.controller_handle,
+						item->info.agent_handle,
+						NULL));
+		}
+		opened_by_driver = false;
+		/* Check if all controllers are disconnected */
+		list_for_each_entry(item, &handler->open_infos, link) {
+			if (item->info.attributes & EFI_OPEN_PROTOCOL_BY_DRIVER)
+				opened_by_driver = true;
+		}
+		/* Only one controller can be conncected */
+		if (opened_by_driver)
+			return EFI_ACCESS_DENIED;
+	}
+
+	/* Find existing entry */
+	list_for_each_entry(item, &handler->open_infos, link) {
+		if (item->info.agent_handle == agent_handle &&
+		    item->info.controller_handle == controller_handle)
+			match = &item->info;
+	}
+	/* None found, create one */
+	if (!match) {
+		match = efi_create_open_info(handler);
+		if (!match)
+			return EFI_OUT_OF_RESOURCES;
+	}
+
+	match->agent_handle = agent_handle;
+	match->controller_handle = controller_handle;
+	match->attributes = attributes;
+	match->open_count++;
+
+out:
+	/* For TEST_PROTOCOL ignore interface attribute. */
+	if (attributes != EFI_OPEN_PROTOCOL_TEST_PROTOCOL)
+		*protocol_interface = handler->protocol_interface;
+
+	return EFI_SUCCESS;
+}
+
 /*
  * Open protocol interface on a handle.
  *
@@ -1868,8 +2386,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,
@@ -1882,8 +2399,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:
@@ -1892,45 +2407,28 @@ static efi_status_t EFIAPI efi_open_protocol(
 	case EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER:
 		if (controller_handle == handle)
 			goto out;
+		/* fall-through */
 	case EFI_OPEN_PROTOCOL_BY_DRIVER:
 	case EFI_OPEN_PROTOCOL_BY_DRIVER | EFI_OPEN_PROTOCOL_EXCLUSIVE:
-		if (controller_handle == NULL)
+		/* Check that the controller handle is valid */
+		if (!efi_search_obj(controller_handle))
 			goto out;
+		/* fall-through */
 	case EFI_OPEN_PROTOCOL_EXCLUSIVE:
-		if (agent_handle == NULL)
+		/* Check that the agent handle is valid */
+		if (!efi_search_obj(agent_handle))
 			goto out;
 		break;
 	default:
 		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;
+	r = efi_protocol_open(handler, protocol_interface, agent_handle,
+			      controller_handle, attributes);
 out:
 	return EFI_EXIT(r);
 }
@@ -1947,7 +2445,7 @@ out:
  * @protocol_interface  interface implementing the protocol
  * @return		status code
  */
-static efi_status_t EFIAPI efi_handle_protocol(void *handle,
+static efi_status_t EFIAPI efi_handle_protocol(efi_handle_t handle,
 					       const efi_guid_t *protocol,
 					       void **protocol_interface)
 {
@@ -1955,6 +2453,321 @@ static efi_status_t EFIAPI efi_handle_protocol(void *handle,
 				 NULL, EFI_OPEN_PROTOCOL_BY_HANDLE_PROTOCOL);
 }
 
+static efi_status_t efi_bind_controller(
+			efi_handle_t controller_handle,
+			efi_handle_t driver_image_handle,
+			struct efi_device_path *remain_device_path)
+{
+	struct efi_driver_binding_protocol *binding_protocol;
+	efi_status_t r;
+
+	r = EFI_CALL(efi_open_protocol(driver_image_handle,
+				       &efi_guid_driver_binding_protocol,
+				       (void **)&binding_protocol,
+				       driver_image_handle, NULL,
+				       EFI_OPEN_PROTOCOL_GET_PROTOCOL));
+	if (r != EFI_SUCCESS)
+		return r;
+	r = EFI_CALL(binding_protocol->supported(binding_protocol,
+						 controller_handle,
+						 remain_device_path));
+	if (r == EFI_SUCCESS)
+		r = EFI_CALL(binding_protocol->start(binding_protocol,
+						     controller_handle,
+						     remain_device_path));
+	EFI_CALL(efi_close_protocol(driver_image_handle,
+				    &efi_guid_driver_binding_protocol,
+				    driver_image_handle, NULL));
+	return r;
+}
+
+static efi_status_t efi_connect_single_controller(
+			efi_handle_t controller_handle,
+			efi_handle_t *driver_image_handle,
+			struct efi_device_path *remain_device_path)
+{
+	efi_handle_t *buffer;
+	size_t count;
+	size_t i;
+	efi_status_t r;
+	size_t connected = 0;
+
+	/* Get buffer with all handles with driver binding protocol */
+	r = EFI_CALL(efi_locate_handle_buffer(BY_PROTOCOL,
+					      &efi_guid_driver_binding_protocol,
+					      NULL, &count, &buffer));
+	if (r != EFI_SUCCESS)
+		return r;
+
+	/*  Context Override */
+	if (driver_image_handle) {
+		for (; *driver_image_handle; ++driver_image_handle) {
+			for (i = 0; i < count; ++i) {
+				if (buffer[i] == *driver_image_handle) {
+					buffer[i] = NULL;
+					r = efi_bind_controller(
+							controller_handle,
+							*driver_image_handle,
+							remain_device_path);
+					/*
+					 * For drivers that do not support the
+					 * controller or are already connected
+					 * we receive an error code here.
+					 */
+					if (r == EFI_SUCCESS)
+						++connected;
+				}
+			}
+		}
+	}
+
+	/*
+	 * TODO: Some overrides are not yet implemented:
+	 * - Platform Driver Override
+	 * - Driver Family Override Search
+	 * - Bus Specific Driver Override
+	 */
+
+	/* Driver Binding Search */
+	for (i = 0; i < count; ++i) {
+		if (buffer[i]) {
+			r = efi_bind_controller(controller_handle,
+						buffer[i],
+						remain_device_path);
+			if (r == EFI_SUCCESS)
+				++connected;
+		}
+	}
+
+	efi_free_pool(buffer);
+	if (!connected)
+		return EFI_NOT_FOUND;
+	return EFI_SUCCESS;
+}
+
+/*
+ * Connect a controller to a driver.
+ *
+ * This function implements the ConnectController service.
+ * See the Unified Extensible Firmware Interface (UEFI) specification
+ * for details.
+ *
+ * First all driver binding protocol handles are tried for binding drivers.
+ * Afterwards all handles that have openened a protocol of the controller
+ * with EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER are connected to drivers.
+ *
+ * @controller_handle	handle of the controller
+ * @driver_image_handle	handle of the driver
+ * @remain_device_path	device path of a child controller
+ * @recursive		true to connect all child controllers
+ * @return		status code
+ */
+static efi_status_t EFIAPI efi_connect_controller(
+			efi_handle_t controller_handle,
+			efi_handle_t *driver_image_handle,
+			struct efi_device_path *remain_device_path,
+			bool recursive)
+{
+	efi_status_t r;
+	efi_status_t ret = EFI_NOT_FOUND;
+	struct efi_object *efiobj;
+
+	EFI_ENTRY("%p, %p, %p, %d", controller_handle, driver_image_handle,
+		  remain_device_path, recursive);
+
+	efiobj = efi_search_obj(controller_handle);
+	if (!efiobj) {
+		ret = EFI_INVALID_PARAMETER;
+		goto out;
+	}
+
+	r = efi_connect_single_controller(controller_handle,
+					  driver_image_handle,
+					  remain_device_path);
+	if (r == EFI_SUCCESS)
+		ret = EFI_SUCCESS;
+	if (recursive) {
+		struct efi_handler *handler;
+		struct efi_open_protocol_info_item *item;
+
+		list_for_each_entry(handler, &efiobj->protocols, link) {
+			list_for_each_entry(item, &handler->open_infos, link) {
+				if (item->info.attributes &
+				    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER) {
+					r = EFI_CALL(efi_connect_controller(
+						item->info.controller_handle,
+						driver_image_handle,
+						remain_device_path,
+						recursive));
+					if (r == EFI_SUCCESS)
+						ret = EFI_SUCCESS;
+				}
+			}
+		}
+	}
+	/*  Check for child controller specified by end node */
+	if (ret != EFI_SUCCESS && remain_device_path &&
+	    remain_device_path->type == DEVICE_PATH_TYPE_END)
+		ret = EFI_SUCCESS;
+out:
+	return EFI_EXIT(ret);
+}
+
+/*
+ * Get all child controllers associated to a driver.
+ * The allocated buffer has to be freed with free().
+ *
+ * @efiobj			handle of the controller
+ * @driver_handle		handle of the driver
+ * @number_of_children		number of child controllers
+ * @child_handle_buffer		handles of the the child controllers
+ */
+static efi_status_t efi_get_child_controllers(
+				struct efi_object *efiobj,
+				efi_handle_t driver_handle,
+				efi_uintn_t *number_of_children,
+				efi_handle_t **child_handle_buffer)
+{
+	struct efi_handler *handler;
+	struct efi_open_protocol_info_item *item;
+	efi_uintn_t count = 0, i;
+	bool duplicate;
+
+	/* Count all child controller associations */
+	list_for_each_entry(handler, &efiobj->protocols, link) {
+		list_for_each_entry(item, &handler->open_infos, link) {
+			if (item->info.agent_handle == driver_handle &&
+			    item->info.attributes &
+			    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER)
+				++count;
+		}
+	}
+	/*
+	 * Create buffer. In case of duplicate child controller assignments
+	 * the buffer will be too large. But that does not harm.
+	 */
+	*number_of_children = 0;
+	*child_handle_buffer = calloc(count, sizeof(efi_handle_t));
+	if (!*child_handle_buffer)
+		return EFI_OUT_OF_RESOURCES;
+	/* Copy unique child handles */
+	list_for_each_entry(handler, &efiobj->protocols, link) {
+		list_for_each_entry(item, &handler->open_infos, link) {
+			if (item->info.agent_handle == driver_handle &&
+			    item->info.attributes &
+			    EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER) {
+				/* Check this is a new child controller */
+				duplicate = false;
+				for (i = 0; i < *number_of_children; ++i) {
+					if ((*child_handle_buffer)[i] ==
+					    item->info.controller_handle)
+						duplicate = true;
+				}
+				/* Copy handle to buffer */
+				if (!duplicate) {
+					i = (*number_of_children)++;
+					(*child_handle_buffer)[i] =
+						item->info.controller_handle;
+				}
+			}
+		}
+	}
+	return EFI_SUCCESS;
+}
+
+/*
+ * Disconnect a controller from a driver.
+ *
+ * This function implements the DisconnectController service.
+ * See the Unified Extensible Firmware Interface (UEFI) specification
+ * for details.
+ *
+ * @controller_handle	handle of the controller
+ * @driver_image_handle handle of the driver
+ * @child_handle	handle of the child to destroy
+ * @return		status code
+ */
+static efi_status_t EFIAPI efi_disconnect_controller(
+				efi_handle_t controller_handle,
+				efi_handle_t driver_image_handle,
+				efi_handle_t child_handle)
+{
+	struct efi_driver_binding_protocol *binding_protocol;
+	efi_handle_t *child_handle_buffer = NULL;
+	size_t number_of_children = 0;
+	efi_status_t r;
+	size_t stop_count = 0;
+	struct efi_object *efiobj;
+
+	EFI_ENTRY("%p, %p, %p", controller_handle, driver_image_handle,
+		  child_handle);
+
+	efiobj = efi_search_obj(controller_handle);
+	if (!efiobj) {
+		r = EFI_INVALID_PARAMETER;
+		goto out;
+	}
+
+	if (child_handle && !efi_search_obj(child_handle)) {
+		r = EFI_INVALID_PARAMETER;
+		goto out;
+	}
+
+	/* If no driver handle is supplied, disconnect all drivers */
+	if (!driver_image_handle) {
+		r = efi_disconnect_all_drivers(efiobj, NULL, child_handle);
+		goto out;
+	}
+
+	/* Create list of child handles */
+	if (child_handle) {
+		number_of_children = 1;
+		child_handle_buffer = &child_handle;
+	} else {
+		efi_get_child_controllers(efiobj,
+					  driver_image_handle,
+					  &number_of_children,
+					  &child_handle_buffer);
+	}
+
+	/* Get the driver binding protocol */
+	r = EFI_CALL(efi_open_protocol(driver_image_handle,
+				       &efi_guid_driver_binding_protocol,
+				       (void **)&binding_protocol,
+				       driver_image_handle, NULL,
+				       EFI_OPEN_PROTOCOL_GET_PROTOCOL));
+	if (r != EFI_SUCCESS)
+		goto out;
+	/* Remove the children */
+	if (number_of_children) {
+		r = EFI_CALL(binding_protocol->stop(binding_protocol,
+						    controller_handle,
+						    number_of_children,
+						    child_handle_buffer));
+		if (r == EFI_SUCCESS)
+			++stop_count;
+	}
+	/* Remove the driver */
+	if (!child_handle)
+		r = EFI_CALL(binding_protocol->stop(binding_protocol,
+						    controller_handle,
+						    0, NULL));
+	if (r == EFI_SUCCESS)
+		++stop_count;
+	EFI_CALL(efi_close_protocol(driver_image_handle,
+				    &efi_guid_driver_binding_protocol,
+				    driver_image_handle, NULL));
+
+	if (stop_count)
+		r = EFI_SUCCESS;
+	else
+		r = EFI_NOT_FOUND;
+out:
+	if (!child_handle)
+		free(child_handle_buffer);
+	return EFI_EXIT(r);
+}
+
 static const struct efi_boot_services efi_boot_services = {
 	.hdr = {
 		.headersize = sizeof(struct efi_table_hdr),
@@ -2005,8 +2818,7 @@ static const struct efi_boot_services efi_boot_services = {
 };
 
 
-static uint16_t __efi_runtime_data firmware_vendor[] =
-	{ 'D','a','s',' ','U','-','b','o','o','t',0 };
+static uint16_t __efi_runtime_data firmware_vendor[] = L"Das U-Boot";
 
 struct efi_system_table __efi_runtime_data systab = {
 	.hdr = {