[U-Boot] [PATCH 15/23] efi_loader: implement ConnectController

[Heinrich Schuchardt <xypron.glpk@gmx.de>]

Signed-off-by: Heinrich Schuchardt <xypron.glpk@gmx.de>
---
 include/efi_api.h             |  22 ++++++++
 include/efi_loader.h          |   1 +
 lib/efi_loader/efi_boottime.c | 119 +++++++++++++++++++++++++++++++++++++++++-
 3 files changed, 141 insertions(+), 1 deletion(-)

diff --git a/include/efi_api.h b/include/efi_api.h
index 8efc8dfab8..b2838125d7 100644
--- a/include/efi_api.h
+++ b/include/efi_api.h
@@ -609,4 +609,26 @@ struct efi_pxe {
 	struct efi_pxe_mode *mode;
 };
 
+#define EFI_DRIVER_BINDING_PROTOCOL_GUID \
+	EFI_GUID(0x18a031ab, 0xb443, 0x4d1a,\
+		 0xa5, 0xc0, 0x0c, 0x09, 0x26, 0x1e, 0x9f, 0x71)
+struct efi_driver_binding_protocol {
+	efi_status_t (EFIAPI * supported)(
+			struct efi_driver_binding_protocol *this,
+			efi_handle_t controller_handle,
+			struct efi_device_path *remaining_device_path);
+	efi_status_t (EFIAPI * start)(
+			struct efi_driver_binding_protocol *this,
+			efi_handle_t controller_handle,
+			struct efi_device_path *remaining_device_path);
+	efi_status_t (EFIAPI * stop)(
+			struct efi_driver_binding_protocol *this,
+			efi_handle_t controller_handle,
+			UINTN number_of_children,
+			efi_handle_t child_handle_buffer);
+	u32 version;
+	efi_handle_t image_handle;
+	efi_handle_t driver_binding_handle;
+};
+
 #endif
diff --git a/include/efi_loader.h b/include/efi_loader.h
index 9c68246c7c..f9f33e1d01 100644
--- a/include/efi_loader.h
+++ b/include/efi_loader.h
@@ -74,6 +74,7 @@ extern const struct efi_device_path_to_text_protocol efi_device_path_to_text;
 
 extern const efi_guid_t efi_guid_console_control;
 extern const efi_guid_t efi_guid_device_path;
+extern const efi_guid_t efi_guid_driver_binding_protocol;
 extern const efi_guid_t efi_guid_loaded_image;
 extern const efi_guid_t efi_guid_device_path_to_text_protocol;
 
diff --git a/lib/efi_loader/efi_boottime.c b/lib/efi_loader/efi_boottime.c
index 5a73ea5cd0..1069da7d79 100644
--- a/lib/efi_loader/efi_boottime.c
+++ b/lib/efi_loader/efi_boottime.c
@@ -18,6 +18,14 @@
 
 DECLARE_GLOBAL_DATA_PTR;
 
+static efi_status_t EFIAPI efi_locate_protocol(const efi_guid_t *protocol,
+					       void *registration,
+					       void **protocol_interface);
+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);
+
 /* This list contains all the EFI objects our payload has access to */
 LIST_HEAD(efi_obj_list);
 
@@ -49,6 +57,9 @@ static struct efi_configuration_table __efi_runtime_data efi_conf_table[2];
 static volatile void *efi_gd, *app_gd;
 #endif
 
+const efi_guid_t efi_guid_driver_binding_protocol =
+			EFI_DRIVER_BINDING_PROTOCOL_GUID;
+
 static int entry_count;
 static int nesting_level;
 
@@ -920,15 +931,121 @@ static efi_status_t EFIAPI efi_set_watchdog_timer(unsigned long timeout,
 	return efi_unsupported(__func__);
 }
 
+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;
+	unsigned long 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);
+					if (r == EFI_SUCCESS)
+						++connected;
+				}
+			}
+		}
+	}
+
+	/*
+	 * Some overrides are not yet implemented:
+	 * Platform Driver Override
+	 * Driver Family Override Search
+	 * 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;
+}
+
 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_ENTRY("%p, %p, %p, %d", controller_handle, driver_image_handle,
 		  remain_device_path, recursive);
-	return EFI_EXIT(EFI_NOT_FOUND);
+
+	if (!controller_handle) {
+		r = EFI_INVALID_PARAMETER;
+		goto out;
+	}
+
+	if (recursive) {
+		r = EFI_UNSUPPORTED;
+		goto out;
+	}
+
+	r = efi_connect_single_controller(controller_handle,
+					  driver_image_handle,
+					  remain_device_path);
+
+out:
+	return EFI_EXIT(r);
 }
 
 static efi_status_t EFIAPI efi_disconnect_controller(void *controller_handle,
-- 
2.14.1