diff --git a/include/efi_loader.h b/include/efi_loader.h
index 553c615f11..222b251a38 100644
--- a/include/efi_loader.h
+++ b/include/efi_loader.h
@@ -88,6 +88,7 @@ extern unsigned int __efi_runtime_rel_start, __efi_runtime_rel_stop;
 struct efi_handler {
 	const efi_guid_t *guid;
 	void *protocol_interface;
+	struct efi_open_protocol_info_entry open_info[4];
 };
 
 /*
diff --git a/lib/efi_loader/efi_boottime.c b/lib/efi_loader/efi_boottime.c
index ebb557abb2..e969814476 100644
--- a/lib/efi_loader/efi_boottime.c
+++ b/lib/efi_loader/efi_boottime.c
@@ -898,23 +898,78 @@ static efi_status_t EFIAPI efi_connect_controller(
 	return EFI_EXIT(EFI_NOT_FOUND);
 }
 
-static efi_status_t EFIAPI efi_disconnect_controller(void *controller_handle,
-						     void *driver_image_handle,
-						     void *child_handle)
+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);
 }
 
+static efi_status_t efi_close_protocol_int(struct efi_handler *protocol,
+					   void *agent_handle,
+					   void *controller_handle)
+{
+	size_t i;
+	struct efi_open_protocol_info_entry *open_info;
+
+	for (i = 0; i < ARRAY_SIZE(protocol->open_info); ++i) {
+		open_info = &protocol->open_info[i];
+
+		if (!open_info->open_count)
+			continue;
+
+		if (open_info->agent_handle == agent_handle &&
+		    open_info->controller_handle ==
+		    controller_handle) {
+			open_info->open_count--;
+			return EFI_SUCCESS;
+		}
+	}
+	return EFI_NOT_FOUND;
+}
+
 static efi_status_t EFIAPI efi_close_protocol(void *handle,
 					      efi_guid_t *protocol,
 					      void *agent_handle,
 					      void *controller_handle)
 {
+	struct efi_object *efiobj;
+	size_t i;
+	efi_status_t r = EFI_NOT_FOUND;
+
 	EFI_ENTRY("%p, %p, %p, %p", handle, protocol, agent_handle,
 		  controller_handle);
-	return EFI_EXIT(EFI_NOT_FOUND);
+
+	if (!handle || !protocol || !agent_handle) {
+		r = EFI_INVALID_PARAMETER;
+		goto out;
+	}
+
+	EFI_PRINT_GUID("protocol:", protocol);
+
+	list_for_each_entry(efiobj, &efi_obj_list, 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)) {
+				r = efi_close_protocol_int(handler,
+							   agent_handle,
+							   controller_handle);
+				goto out;
+			}
+		}
+		goto out;
+	}
+out:
+	return EFI_EXIT(r);
 }
 
 static efi_status_t EFIAPI efi_open_protocol_information(efi_handle_t handle,
@@ -1119,6 +1174,96 @@ static void EFIAPI efi_set_mem(void *buffer, unsigned long size, uint8_t value)
 	memset(buffer, value, size);
 }
 
+static efi_status_t efi_open_protocol_int(
+			struct efi_handler *protocol,
+			void **protocol_interface, void *agent_handle,
+			void *controller_handle, uint32_t attributes)
+{
+	bool opened_exclusive = false;
+	bool opened_by_driver = false;
+	int i;
+	struct efi_open_protocol_info_entry *open_info;
+	struct efi_open_protocol_info_entry *match = NULL;
+
+	if (attributes !=
+	    EFI_OPEN_PROTOCOL_TEST_PROTOCOL) {
+		*protocol_interface = NULL;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(protocol->open_info); ++i) {
+		open_info = &protocol->open_info[i];
+
+		if (!open_info->open_count)
+			continue;
+		if (open_info->agent_handle == agent_handle) {
+			if ((attributes & EFI_OPEN_PROTOCOL_BY_DRIVER) &&
+			    (open_info->attributes == attributes))
+				return EFI_ALREADY_STARTED;
+			if (open_info->controller_handle == controller_handle)
+				match = open_info;
+		}
+		if (open_info->attributes & EFI_OPEN_PROTOCOL_EXCLUSIVE)
+			opened_exclusive = true;
+	}
+
+	if (attributes &
+	    (EFI_OPEN_PROTOCOL_EXCLUSIVE | EFI_OPEN_PROTOCOL_BY_DRIVER) &&
+	    opened_exclusive)
+		return EFI_ACCESS_DENIED;
+
+	if (attributes & EFI_OPEN_PROTOCOL_EXCLUSIVE) {
+		for (i = 0; i < ARRAY_SIZE(protocol->open_info); ++i) {
+			open_info = &protocol->open_info[i];
+
+			if (!open_info->open_count)
+				continue;
+			if (open_info->attributes ==
+					EFI_OPEN_PROTOCOL_BY_DRIVER)
+				EFI_CALL(efi_disconnect_controller(
+						open_info->controller_handle,
+						open_info->agent_handle,
+						NULL));
+		}
+		opened_by_driver = false;
+		for (i = 0; i < ARRAY_SIZE(protocol->open_info); ++i) {
+			open_info = &protocol->open_info[i];
+
+			if (!open_info->open_count)
+				continue;
+			if (open_info->attributes & EFI_OPEN_PROTOCOL_BY_DRIVER)
+				opened_by_driver = true;
+		}
+		if (opened_by_driver)
+			return EFI_ACCESS_DENIED;
+		if (match && !match->open_count)
+			match = NULL;
+	}
+
+	/*
+	 * Find an empty slot.
+	 */
+	if (!match) {
+		for (i = 0; i < ARRAY_SIZE(protocol->open_info); ++i) {
+			open_info = &protocol->open_info[i];
+
+			if (!open_info->open_count) {
+				match = open_info;
+				break;
+			}
+		}
+	}
+	if (!match)
+		return EFI_OUT_OF_RESOURCES;
+
+	match->agent_handle = agent_handle;
+	match->controller_handle = controller_handle;
+	match->attributes = attributes;
+	match->open_count++;
+	*protocol_interface = protocol->protocol_interface;
+
+	return EFI_SUCCESS;
+}
+
 static efi_status_t EFIAPI efi_open_protocol(
 			void *handle, efi_guid_t *protocol,
 			void **protocol_interface, void *agent_handle,
@@ -1173,12 +1318,11 @@ static efi_status_t EFIAPI efi_open_protocol(
 			if (!hprotocol)
 				continue;
 			if (!guidcmp(hprotocol, protocol)) {
-				if (attributes !=
-				    EFI_OPEN_PROTOCOL_TEST_PROTOCOL) {
-					*protocol_interface =
-						handler->protocol_interface;
-				}
-				r = EFI_SUCCESS;
+				r = efi_open_protocol_int(handler,
+							  protocol_interface,
+							  agent_handle,
+							  controller_handle,
+							  attributes);
 				goto out;
 			}
 		}
