1 files changed, 53 insertions, 23 deletions

diff --git a/lib/efi_loader/efi_boottime.c b/lib/efi_loader/efi_boottime.c
index d5065f296a..0e89c8505b 100644
--- a/lib/efi_loader/efi_boottime.c
+++ b/lib/efi_loader/efi_boottime.c
@@ -59,6 +59,10 @@ static efi_handle_t current_image;
 static volatile gd_t *efi_gd, *app_gd;
 #endif
 
+static efi_status_t efi_uninstall_protocol
+			(efi_handle_t handle, const efi_guid_t *protocol,
+			 void *protocol_interface);
+
 /* 1 if inside U-Boot code, 0 if inside EFI payload code */
 static int entry_count = 1;
 static int nesting_level;
@@ -97,6 +101,12 @@ static efi_status_t EFIAPI efi_disconnect_controller(
 					efi_handle_t driver_image_handle,
 					efi_handle_t child_handle);
 
+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);
+
 /* Called on every callback entry */
 int __efi_entry_check(void)
 {
@@ -569,9 +579,9 @@ efi_status_t efi_search_protocol(const efi_handle_t handle,
  *
  * Return: status code
  */
-efi_status_t efi_remove_protocol(const efi_handle_t handle,
-				 const efi_guid_t *protocol,
-				 void *protocol_interface)
+static 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;
@@ -604,8 +614,8 @@ static efi_status_t efi_remove_all_protocols(const efi_handle_t handle)
 	list_for_each_entry_safe(protocol, pos, &efiobj->protocols, link) {
 		efi_status_t ret;
 
-		ret = efi_remove_protocol(handle, &protocol->guid,
-					  protocol->protocol_interface);
+		ret = efi_uninstall_protocol(handle, &protocol->guid,
+					     protocol->protocol_interface);
 		if (ret != EFI_SUCCESS)
 			return ret;
 	}
@@ -616,19 +626,23 @@ static efi_status_t efi_remove_all_protocols(const efi_handle_t handle)
  * efi_delete_handle() - delete handle
  *
  * @handle: handle to delete
+ *
+ * Return: status code
  */
-void efi_delete_handle(efi_handle_t handle)
+efi_status_t efi_delete_handle(efi_handle_t handle)
 {
 	efi_status_t ret;
 
 	ret = efi_remove_all_protocols(handle);
-	if (ret == EFI_INVALID_PARAMETER) {
-		log_err("Can't remove invalid handle %p\n", handle);
-		return;
+	if (ret != EFI_SUCCESS) {
+		log_err("Handle %p has protocols installed. Unable to delete\n", handle);
+		return ret;
 	}
 
 	list_del(&handle->link);
 	free(handle);
+
+	return ret;
 }
 
 /**
@@ -1298,7 +1312,7 @@ static efi_status_t efi_disconnect_all_drivers
 				 const efi_guid_t *protocol,
 				 efi_handle_t child_handle)
 {
-	efi_uintn_t number_of_drivers;
+	efi_uintn_t number_of_drivers, tmp;
 	efi_handle_t *driver_handle_buffer;
 	efi_status_t r, ret;
 
@@ -1308,15 +1322,30 @@ static efi_status_t efi_disconnect_all_drivers
 		return ret;
 	if (!number_of_drivers)
 		return EFI_SUCCESS;
-	ret = EFI_NOT_FOUND;
+
+	tmp = number_of_drivers;
 	while (number_of_drivers) {
-		r = EFI_CALL(efi_disconnect_controller(
+		ret = EFI_CALL(efi_disconnect_controller(
 				handle,
 				driver_handle_buffer[--number_of_drivers],
 				child_handle));
-		if (r == EFI_SUCCESS)
-			ret = r;
+		if (ret != EFI_SUCCESS)
+			goto reconnect;
 	}
+
+	free(driver_handle_buffer);
+	return ret;
+
+reconnect:
+	/* Reconnect all disconnected drivers */
+	for (; number_of_drivers < tmp; number_of_drivers++) {
+		r = EFI_CALL(efi_connect_controller(handle,
+						    &driver_handle_buffer[number_of_drivers],
+						    NULL, true));
+		if (r != EFI_SUCCESS)
+			EFI_PRINT("Failed to reconnect controller\n");
+	}
+
 	free(driver_handle_buffer);
 	return ret;
 }
@@ -1336,34 +1365,35 @@ static efi_status_t efi_uninstall_protocol
 			(efi_handle_t handle, const efi_guid_t *protocol,
 			 void *protocol_interface)
 {
-	struct efi_object *efiobj;
 	struct efi_handler *handler;
 	struct efi_open_protocol_info_item *item;
 	struct efi_open_protocol_info_item *pos;
 	efi_status_t r;
 
-	/* Check handle */
-	efiobj = efi_search_obj(handle);
-	if (!efiobj) {
-		r = EFI_INVALID_PARAMETER;
-		goto out;
-	}
 	/* Find the protocol on the handle */
 	r = efi_search_protocol(handle, protocol, &handler);
 	if (r != EFI_SUCCESS)
 		goto out;
+	if (handler->protocol_interface != protocol_interface)
+		return EFI_NOT_FOUND;
 	/* Disconnect controllers */
-	efi_disconnect_all_drivers(efiobj, protocol, NULL);
+	r = efi_disconnect_all_drivers(handle, protocol, NULL);
+	if (r != EFI_SUCCESS) {
+		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);
+			efi_delete_open_info(item);
 	}
+	/* if agents didn't close the protocols properly */
 	if (!list_empty(&handler->open_infos)) {
 		r =  EFI_ACCESS_DENIED;
+		EFI_CALL(efi_connect_controller(handle, NULL, NULL, true));
 		goto out;
 	}
 	r = efi_remove_protocol(handle, protocol, protocol_interface);