efi_boottime.c

	case BY_REGISTER_NOTIFY:
		/* TODO: RegisterProtocolNotify is not implemented yet */
		return -1;
	case BY_PROTOCOL:
		ret = efi_search_protocol(efiobj->handle, protocol, NULL);
		return (ret != EFI_SUCCESS);
	default:
		/* Invalid search type */
		return -1;
	}
}

/*
 * Locate handles implementing a protocol.
 *
 * This function is meant for U-Boot internal calls. For the API implementation
 * of the LocateHandle service see efi_locate_handle_ext.
 *
 * @search_type		selection criterion
 * @protocol		GUID of the protocol
 * @search_key		registration key
 * @buffer_size		size of the buffer to receive the handles in bytes
 * @buffer		buffer to receive the relevant handles
 * @return		status code
 */
static efi_status_t efi_locate_handle(
			enum efi_locate_search_type search_type,
			const efi_guid_t *protocol, void *search_key,
			efi_uintn_t *buffer_size, efi_handle_t *buffer)
{
	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_entry(efiobj, &efi_obj_list, link) {
		if (!efi_search(search_type, protocol, search_key, efiobj))
			size += sizeof(void*);
	}

	if (*buffer_size < size) {
		*buffer_size = size;
		return EFI_BUFFER_TOO_SMALL;
	}

	*buffer_size = size;
	if (size == 0)
		return EFI_NOT_FOUND;

	/* Then fill the array */
	list_for_each_entry(efiobj, &efi_obj_list, link) {
		if (!efi_search(search_type, protocol, search_key, efiobj))
			*buffer++ = efiobj->handle;
	}

	return EFI_SUCCESS;
}

/*
 * Locate handles implementing a protocol.
 *
 * This function implements the LocateHandle service.
 * See the Unified Extensible Firmware Interface (UEFI) specification
 * for details.
 *
 * @search_type		selection criterion
 * @protocol		GUID of the protocol
 * @search_key		registration key
 * @buffer_size		size of the buffer to receive the handles in bytes
 * @buffer		buffer to receive the relevant handles
 * @return		0 if the handle implements the protocol
 */
static efi_status_t EFIAPI efi_locate_handle_ext(
			enum efi_locate_search_type search_type,
			const efi_guid_t *protocol, void *search_key,
			efi_uintn_t *buffer_size, efi_handle_t *buffer)
{
	EFI_ENTRY("%d, %pUl, %p, %p, %p", search_type, protocol, search_key,
		  buffer_size, buffer);

	return EFI_EXIT(efi_locate_handle(search_type, protocol, search_key,
			buffer_size, buffer));
}

/* Collapses configuration table entries, removing index i */
static void efi_remove_configuration_table(int i)
{
	struct efi_configuration_table *this = &efi_conf_table[i];
	struct efi_configuration_table *next = &efi_conf_table[i+1];
	struct efi_configuration_table *end = &efi_conf_table[systab.nr_tables];

	memmove(this, next, (ulong)end - (ulong)next);
	systab.nr_tables--;
}

/*
 * Adds, updates, or removes a configuration table.
 *
 * This function is used for internal calls. For the API implementation of the
 * InstallConfigurationTable service see efi_install_configuration_table_ext.
 *
 * @guid		GUID of the installed table
 * @table		table to be installed
 * @return		status code
 */
efi_status_t efi_install_configuration_table(const efi_guid_t *guid, void *table)
{
	int i;

	/* Check for guid override */
	for (i = 0; i < systab.nr_tables; i++) {
		if (!guidcmp(guid, &efi_conf_table[i].guid)) {
			if (table)
				efi_conf_table[i].table = table;
			else
				efi_remove_configuration_table(i);
			return EFI_SUCCESS;
		}
	}

	if (!table)
		return EFI_NOT_FOUND;

	/* No override, check for overflow */
	if (i >= ARRAY_SIZE(efi_conf_table))
		return EFI_OUT_OF_RESOURCES;

	/* Add a new entry */
	memcpy(&efi_conf_table[i].guid, guid, sizeof(*guid));
	efi_conf_table[i].table = table;
	systab.nr_tables = i + 1;

	return EFI_SUCCESS;
}

/*
 * Adds, updates, or removes a configuration table.
 *
 * This function implements the InstallConfigurationTable service.
 * See the Unified Extensible Firmware Interface (UEFI) specification
 * for details.
 *
 * @guid		GUID of the installed table
 * @table		table to be installed
 * @return		status code
 */
static efi_status_t EFIAPI efi_install_configuration_table_ext(efi_guid_t *guid,
							       void *table)
{
	EFI_ENTRY("%pUl, %p", guid, table);
	return EFI_EXIT(efi_install_configuration_table(guid, table));
}

/*
 * Initialize a loaded_image_info + loaded_image_info object with correct
 * protocols, boot-device, etc.
 *
 * @info		loaded image info to be passed to the entry point of the
 *			image
 * @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
 */
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
	 */
	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
	 */
	ret = efi_add_protocol(obj->handle, &efi_guid_loaded_image, info);
	if (ret != EFI_SUCCESS)
		goto failure;

	ret = efi_add_protocol(obj->handle, &efi_guid_console_control,
			       (void *)&efi_console_control);
	if (ret != EFI_SUCCESS)
		goto failure;

	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;

	return ret;
failure:
	printf("ERROR: Failure to install protocols for loaded image\n");
	return ret;
}

/*
 * Load an image using a file path.
 *
 * @file_path		the path of the image to load
 * @buffer		buffer containing the loaded image
 * @return		status code
 */
efi_status_t efi_load_image_from_path(struct efi_device_path *file_path,
				      void **buffer)
{
	struct efi_file_info *info = NULL;
	struct efi_file_handle *f;
	static efi_status_t ret;
	uint64_t bs;

	f = efi_file_from_path(file_path);
	if (!f)
		return EFI_DEVICE_ERROR;

	bs = 0;
	EFI_CALL(ret = f->getinfo(f, (efi_guid_t *)&efi_file_info_guid,
				  &bs, info));
	if (ret == EFI_BUFFER_TOO_SMALL) {
		info = malloc(bs);
		EFI_CALL(ret = f->getinfo(f, (efi_guid_t *)&efi_file_info_guid,
					  &bs, info));
	}
	if (ret != EFI_SUCCESS)
		goto error;

	ret = efi_allocate_pool(EFI_LOADER_DATA, info->file_size, buffer);
	if (ret)
		goto error;

	EFI_CALL(ret = f->read(f, &info->file_size, *buffer));

error:
	free(info);
	EFI_CALL(f->close(f));

	if (ret != EFI_SUCCESS) {
		efi_free_pool(*buffer);
		*buffer = NULL;
	}

	return ret;
}

/*
 * Load an EFI image into memory.
 *
 * This function implements the LoadImage service.
 * See the Unified Extensible Firmware Interface (UEFI) specification
 * for details.
 *
 * @boot_policy		true for request originating from the boot manager
 * @parent_image	the calles's image handle
 * @file_path		the path of the image to load
 * @source_buffer	memory location from which the image is installed
 * @source_size		size of the memory area from which the image is
 *			installed
 * @image_handle	handle for the newly installed image
 * @return		status code
 */
static efi_status_t EFIAPI efi_load_image(bool boot_policy,
					  efi_handle_t parent_image,
					  struct efi_device_path *file_path,
					  void *source_buffer,
					  unsigned long source_size,
					  efi_handle_t *image_handle)
{
	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,
		  file_path, source_buffer, source_size, image_handle);

	info = calloc(1, sizeof(*info));
	obj = calloc(1, sizeof(*obj));

	if (!source_buffer) {
		struct efi_device_path *dp, *fp;

		ret = efi_load_image_from_path(file_path, &source_buffer);
		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);
		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
		 */
		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) {
		ret = EFI_UNSUPPORTED;
		goto failure;
	}
	info->system_table = &systab;
	info->parent_handle = parent_image;
	*image_handle = obj->handle;
	return EFI_EXIT(EFI_SUCCESS);
failure:
	free(info);
	efi_delete_handle(obj);
	return EFI_EXIT(ret);
}

/*
 * Call the entry point of an image.
 *
 * This function implements the StartImage service.
 * See the Unified Extensible Firmware Interface (UEFI) specification
 * for details.
 *
 * @image_handle	handle of the image
 * @exit_data_size	size of the buffer
 * @exit_data		buffer to receive the exit data of the called image
 * @return		status code
 */
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);
	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;

	efi_is_direct_boot = false;

	/* call the image! */
	if (setjmp(&info->exit_jmp)) {
		/*
		 * 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);
	}

	ret = EFI_CALL(entry(image_handle, &systab));

	/* Should usually never get here */
	return EFI_EXIT(ret);
}

/*
 * Leave an EFI application or driver.
 *
 * This function implements the Exit service.
 * See the Unified Extensible Firmware Interface (UEFI) specification
 * for details.
 *
 * @image_handle	handle of the application or driver that is exiting
 * @exit_status		status code
 * @exit_data_size	size of the buffer in bytes
 * @exit_data		buffer with data describing an error
 * @return		status code
 */
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(exit_status);

	/*
	 * But longjmp out with the U-Boot gd, not the application's, as
	 * the other end is a setjmp call inside EFI context.
	 */
	efi_restore_gd();

	loaded_image_info->exit_status = exit_status;
	longjmp(&loaded_image_info->exit_jmp, 1);

	panic("EFI application exited");
}

/*
 * Unload an EFI image.
 *
 * This function implements the UnloadImage service.
 * See the Unified Extensible Firmware Interface (UEFI) specification
 * for details.
 *
 * @image_handle	handle of the image to be unloaded
 * @return		status code
 */
static efi_status_t EFIAPI efi_unload_image(void *image_handle)
{
	struct efi_object *efiobj;

	EFI_ENTRY("%p", image_handle);
	efiobj = efi_search_obj(image_handle);
	if (efiobj)
		list_del(&efiobj->link);

	return EFI_EXIT(EFI_SUCCESS);
}

/*
 * Fix up caches for EFI payloads if necessary.
 */
static void efi_exit_caches(void)
{
#if defined(CONFIG_ARM) && !defined(CONFIG_ARM64)
	/*
	 * Grub on 32bit ARM needs to have caches disabled before jumping into
	 * a zImage, but does not know of all cache layers. Give it a hand.
	 */
	if (efi_is_direct_boot)
		cleanup_before_linux();
#endif
}

/*
 * Stop boot services.
 *
 * This function implements the ExitBootServices service.
 * See the Unified Extensible Firmware Interface (UEFI) specification
 * for details.
 *
 * @image_handle	handle of the loaded image
 * @map_key		key of the memory map
 * @return		status code
 */
static efi_status_t EFIAPI efi_exit_boot_services(void *image_handle,
						  unsigned long map_key)
{
	int i;

	EFI_ENTRY("%p, %ld", image_handle, map_key);

	/* Notify that ExitBootServices is invoked. */
	for (i = 0; i < ARRAY_SIZE(efi_events); ++i) {
		if (efi_events[i].type != EVT_SIGNAL_EXIT_BOOT_SERVICES)
			continue;
		efi_signal_event(&efi_events[i]);
	}
	/* Make sure that notification functions are not called anymore */
	efi_tpl = TPL_HIGH_LEVEL;

	/* XXX Should persist EFI variables here */

	board_quiesce_devices();

	/* Fix up caches for EFI payloads if necessary */
	efi_exit_caches();

	/* This stops all lingering devices */
	bootm_disable_interrupts();

	/* Give the payload some time to boot */
	efi_set_watchdog(0);
	WATCHDOG_RESET();

	return EFI_EXIT(EFI_SUCCESS);
}

/*
 * Get next value of the counter.
 *
 * This function implements the NextMonotonicCount service.
 * See the Unified Extensible Firmware Interface (UEFI) specification
 * for details.
 *
 * @count	returned value of the counter
 * @return	status code
 */
static efi_status_t EFIAPI efi_get_next_monotonic_count(uint64_t *count)
{
	static uint64_t mono = 0;
	EFI_ENTRY("%p", count);
	*count = mono++;
	return EFI_EXIT(EFI_SUCCESS);
}

/*
 * Sleep.
 *
 * This function implements the Stall sercive.
 * See the Unified Extensible Firmware Interface (UEFI) specification
 * for details.
 *
 * @microseconds	period to sleep in microseconds
 * @return		status code
 */
static efi_status_t EFIAPI efi_stall(unsigned long microseconds)
{
	EFI_ENTRY("%ld", microseconds);
	udelay(microseconds);
	return EFI_EXIT(EFI_SUCCESS);
}

/*
 * Reset the watchdog timer.
 *
 * This function implements the SetWatchdogTimer service.
 * See the Unified Extensible Firmware Interface (UEFI) specification
 * for details.
 *
 * @timeout		seconds before reset by watchdog
 * @watchdog_code	code to be logged when resetting
 * @data_size		size of buffer in bytes
 * @watchdog_data	buffer with data describing the reset reason
 * @return		status code
 */
static efi_status_t EFIAPI efi_set_watchdog_timer(unsigned long timeout,
						  uint64_t watchdog_code,
						  unsigned long data_size,
						  uint16_t *watchdog_data)
{
	EFI_ENTRY("%ld, 0x%"PRIx64", %ld, %p", timeout, watchdog_code,
		  data_size, watchdog_data);
	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.
 *
 * This function implements the CloseProtocol service.
 * See the Unified Extensible Firmware Interface (UEFI) specification
 * for details.
 *
 * @handle		handle on which the protocol shall be closed
 * @protocol		GUID of the protocol to close
 * @agent_handle	handle of the driver
 * @controller_handle	handle of the controller
 * @return		status code
 */
static efi_status_t EFIAPI efi_close_protocol(void *handle,
					      const efi_guid_t *protocol,
					      void *agent_handle,
					      void *controller_handle)
{
	EFI_ENTRY("%p, %pUl, %p, %p", handle, protocol, agent_handle,
		  controller_handle);
	return EFI_EXIT(EFI_NOT_FOUND);
}

/*
 * Provide information about then open status of a protocol on a handle
 *
 * This function implements the OpenProtocolInformation service.
 * See the Unified Extensible Firmware Interface (UEFI) specification
 * for details.
 *
 * @handle		handle for which the information shall be retrieved
 * @protocol		GUID of the protocol
 * @entry_buffer	buffer to receive the open protocol information
 * @entry_count		number of entries available in the buffer
 * @return		status code
 */
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,
			efi_uintn_t *entry_count)
{
	EFI_ENTRY("%p, %pUl, %p, %p", handle, protocol, entry_buffer,
		  entry_count);
	return EFI_EXIT(EFI_NOT_FOUND);
}

/*
 * Get protocols installed on a handle.
 *
 * This function implements the ProtocolsPerHandleService.
 * See the Unified Extensible Firmware Interface (UEFI) specification
 * for details.
 *
 * @handle			handle for which the information is retrieved
 * @protocol_buffer		buffer with protocol GUIDs
 * @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,
			efi_uintn_t *protocol_buffer_count)
{
	unsigned long buffer_size;
	struct efi_object *efiobj;
	struct list_head *protocol_handle;
	efi_status_t r;

	EFI_ENTRY("%p, %p, %p", handle, protocol_buffer,
		  protocol_buffer_count);

	if (!handle || !protocol_buffer || !protocol_buffer_count)
		return EFI_EXIT(EFI_INVALID_PARAMETER);

	*protocol_buffer = NULL;
	*protocol_buffer_count = 0;

	efiobj = efi_search_obj(handle);
	if (!efiobj)
		return EFI_EXIT(EFI_INVALID_PARAMETER);

	/* 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;
		}
	}

	return EFI_EXIT(EFI_SUCCESS);
}

/*
 * Locate handles implementing a protocol.
 *
 * This function implements the LocateHandleBuffer service.
 * See the Unified Extensible Firmware Interface (UEFI) specification
 * for details.
 *
 * @search_type		selection criterion
 * @protocol		GUID of the protocol
 * @search_key		registration key
 * @no_handles		number of returned handles
 * @buffer		buffer with the returned handles
 * @return		status code
 */
static efi_status_t EFIAPI efi_locate_handle_buffer(
			enum efi_locate_search_type search_type,
			const efi_guid_t *protocol, void *search_key,
			efi_uintn_t *no_handles, efi_handle_t **buffer)
{
	efi_status_t r;
	efi_uintn_t buffer_size = 0;

	EFI_ENTRY("%d, %pUl, %p, %p, %p", search_type, protocol, search_key,
		  no_handles, buffer);

	if (!no_handles || !buffer) {
		r = EFI_INVALID_PARAMETER;
		goto out;
	}
	*no_handles = 0;
	*buffer = NULL;
	r = efi_locate_handle(search_type, protocol, search_key, &buffer_size,
			      *buffer);
	if (r != EFI_BUFFER_TOO_SMALL)
		goto out;
	r = efi_allocate_pool(EFI_ALLOCATE_ANY_PAGES, buffer_size,
			      (void **)buffer);
	if (r != EFI_SUCCESS)
		goto out;
	r = efi_locate_handle(search_type, protocol, search_key, &buffer_size,
			      *buffer);
	if (r == EFI_SUCCESS)
		*no_handles = buffer_size / sizeof(void *);
out:
	return EFI_EXIT(r);
}

/*
 * Find an interface implementing a protocol.
 *
 * This function implements the LocateProtocol service.
 * See the Unified Extensible Firmware Interface (UEFI) specification
 * for details.
 *
 * @protocol		GUID of the protocol
 * @registration	registration key passed to the notification function
 * @protocol_interface	interface implementing the protocol
 * @return		status code
 */
static efi_status_t EFIAPI efi_locate_protocol(const efi_guid_t *protocol,
					       void *registration,
					       void **protocol_interface)
{
	struct list_head *lhandle;
	efi_status_t ret;

	EFI_ENTRY("%pUl, %p, %p", protocol, registration, protocol_interface);

	if (!protocol || !protocol_interface)
		return EFI_EXIT(EFI_INVALID_PARAMETER);

	list_for_each(lhandle, &efi_obj_list) {
		struct efi_object *efiobj;
		struct efi_handler *handler;

		efiobj = list_entry(lhandle, struct efi_object, link);

		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;

	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.
 *
 * This function implements the MultipleProtocolInterfaces service.
 * See the Unified Extensible Firmware Interface (UEFI) specification
 * for details.
 *
 * @handle	handle on which the protocol interfaces shall be installed
 * @...		NULL terminated argument list with pairs of protocol GUIDS and
 *		interfaces
 * @return	status code
 */
static efi_status_t EFIAPI efi_install_multiple_protocol_interfaces(
			void **handle, ...)
{
	EFI_ENTRY("%p", handle);

	va_list argptr;
	const efi_guid_t *protocol;
	void *protocol_interface;
	efi_status_t r = EFI_SUCCESS;
	int 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_install_protocol_interface(
						handle, protocol,
						EFI_NATIVE_INTERFACE,
						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_uninstall_protocol_interface(handle, protocol,
							  protocol_interface));
	}
	va_end(argptr);

	return EFI_EXIT(r);
}

/*
 * Uninstall multiple protocol interfaces.
 *
 * This function implements the UninstallMultipleProtocolInterfaces service.
 * See the Unified Extensible Firmware Interface (UEFI) specification
 * for details.
 *
 * @handle	handle from which the protocol interfaces shall be removed
 * @...		NULL terminated argument list with pairs of protocol GUIDS and
 *		interfaces
 * @return	status code
 */
static efi_status_t EFIAPI efi_uninstall_multiple_protocol_interfaces(
			void *handle, ...)
{
	EFI_ENTRY("%p", handle);

	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);