efi_loader: complete EFI_DEVICE_PATH_UTILITIES_PROTOCOL

The missing services of the EFI_DEVICE_PATH_UTILITIES_PROTOCOL are
implemented.

Signed-off-by: Heinrich Schuchardt <xypron.glpk@gmx.de>
Signed-off-by: Alexander Graf <agraf@suse.de>

2 files changed, 170 insertions, 10 deletions

diff --git a/lib/efi_loader/efi_device_path.c b/lib/efi_loader/efi_device_path.c
index ada0a9c268..634dacf41b 100644
--- a/lib/efi_loader/efi_device_path.c
+++ b/lib/efi_loader/efi_device_path.c
@@ -351,6 +351,70 @@ struct efi_device_path *efi_dp_create_device_node(const u8 type,
 	return ret;
 }
 
+struct efi_device_path *efi_dp_append_instance(
+		const struct efi_device_path *dp,
+		const struct efi_device_path *dpi)
+{
+	size_t sz, szi;
+	struct efi_device_path *p, *ret;
+
+	if (!dpi)
+		return NULL;
+	if (!dp)
+		return efi_dp_dup(dpi);
+	sz = efi_dp_size(dp);
+	szi = efi_dp_instance_size(dpi);
+	p = dp_alloc(sz + szi + 2 * sizeof(END));
+	if (!p)
+		return NULL;
+	ret = p;
+	memcpy(p, dp, sz + sizeof(END));
+	p = (void *)p + sz;
+	p->sub_type = DEVICE_PATH_SUB_TYPE_INSTANCE_END;
+	p = (void *)p + sizeof(END);
+	memcpy(p, dpi, szi);
+	p = (void *)p + szi;
+	memcpy(p, &END, sizeof(END));
+	return ret;
+}
+
+struct efi_device_path *efi_dp_get_next_instance(struct efi_device_path **dp,
+						 efi_uintn_t *size)
+{
+	size_t sz;
+	struct efi_device_path *p;
+
+	if (size)
+		*size = 0;
+	if (!dp || !*dp)
+		return NULL;
+	p = *dp;
+	sz = efi_dp_instance_size(*dp);
+	p = dp_alloc(sz + sizeof(END));
+	if (!p)
+		return NULL;
+	memcpy(p, *dp, sz + sizeof(END));
+	*dp = (void *)*dp + sz;
+	if ((*dp)->sub_type == DEVICE_PATH_SUB_TYPE_INSTANCE_END)
+		*dp = (void *)*dp + sizeof(END);
+	else
+		*dp = NULL;
+	if (size)
+		*size = sz + sizeof(END);
+	return p;
+}
+
+bool efi_dp_is_multi_instance(const struct efi_device_path *dp)
+{
+	const struct efi_device_path *p = dp;
+
+	if (!p)
+		return false;
+	while (p->type != DEVICE_PATH_TYPE_END)
+		p = (void *)p + p->length;
+	return p->sub_type == DEVICE_PATH_SUB_TYPE_INSTANCE_END;
+}
+
 #ifdef CONFIG_DM
 /* size of device-path not including END node for device and all parents
  * up to the root device.
diff --git a/lib/efi_loader/efi_device_path_utilities.c b/lib/efi_loader/efi_device_path_utilities.c
index e73188b242..0ada2111db 100644
--- a/lib/efi_loader/efi_device_path_utilities.c
+++ b/lib/efi_loader/efi_device_path_utilities.c
@@ -12,62 +12,158 @@
 const efi_guid_t efi_guid_device_path_utilities_protocol =
 		EFI_DEVICE_PATH_UTILITIES_PROTOCOL_GUID;
 
+/*
+ * Get size of a device path.
+ *
+ * This function implements the GetDevicePathSize service of the device path
+ * utilities protocol. The device path length includes the end of path tag
+ * which may be an instance end.
+ *
+ * See the Unified Extensible Firmware Interface (UEFI) specification
+ * for details.
+ *
+ * @device_path		device path
+ * @return		size in bytes
+ */
 static efi_uintn_t EFIAPI get_device_path_size(
 	const struct efi_device_path *device_path)
 {
 	efi_uintn_t sz = 0;
 
-	EFI_ENTRY("%p", device_path);
+	EFI_ENTRY("%pD", device_path);
 	/* size includes the END node: */
 	if (device_path)
 		sz = efi_dp_size(device_path) + sizeof(struct efi_device_path);
 	return EFI_EXIT(sz);
 }
 
+/*
+ * Duplicate a device path.
+ *
+ * This function implements the DuplicateDevicePath service of the device path
+ * utilities protocol.
+ *
+ * The UEFI spec does not indicate what happens to the end tag. We follow the
+ * EDK2 logic: In case the device path ends with an end of instance tag, the
+ * copy will also end with an end of instance tag.
+ *
+ * See the Unified Extensible Firmware Interface (UEFI) specification
+ * for details.
+ *
+ * @device_path		device path
+ * @return		copy of the device path
+ */
 static struct efi_device_path * EFIAPI duplicate_device_path(
 	const struct efi_device_path *device_path)
 {
-	EFI_ENTRY("%p", device_path);
+	EFI_ENTRY("%pD", device_path);
 	return EFI_EXIT(efi_dp_dup(device_path));
 }
 
+/*
+ * Append device path.
+ *
+ * This function implements the AppendDevicePath service of the device path
+ * utilities protocol.
+ *
+ * See the Unified Extensible Firmware Interface (UEFI) specification
+ * for details.
+ *
+ * @src1		1st device path
+ * @src2		2nd device path
+ * @return		concatenated device path
+ */
 static struct efi_device_path * EFIAPI append_device_path(
 	const struct efi_device_path *src1,
 	const struct efi_device_path *src2)
 {
-	EFI_ENTRY("%p, %p", src1, src2);
+	EFI_ENTRY("%pD, %pD", src1, src2);
 	return EFI_EXIT(efi_dp_append(src1, src2));
 }
 
+/*
+ * Append device path node.
+ *
+ * This function implements the AppendDeviceNode service of the device path
+ * utilities protocol.
+ *
+ * See the Unified Extensible Firmware Interface (UEFI) specification
+ * for details.
+ *
+ * @device_path		device path
+ * @device_node		device node
+ * @return		concatenated device path
+ */
 static struct efi_device_path * EFIAPI append_device_node(
 	const struct efi_device_path *device_path,
 	const struct efi_device_path *device_node)
 {
-	EFI_ENTRY("%p, %p", device_path, device_node);
+	EFI_ENTRY("%pD, %p", device_path, device_node);
 	return EFI_EXIT(efi_dp_append_node(device_path, device_node));
 }
 
+/*
+ * Append device path instance.
+ *
+ * This function implements the AppendDevicePathInstance service of the device
+ * path utilities protocol.
+ *
+ * See the Unified Extensible Firmware Interface (UEFI) specification
+ * for details.
+ *
+ * @device_path			1st device path
+ * @device_path_instance	2nd device path
+ * @return			concatenated device path
+ */
 static struct efi_device_path * EFIAPI append_device_path_instance(
 	const struct efi_device_path *device_path,
 	const struct efi_device_path *device_path_instance)
 {
-	EFI_ENTRY("%p, %p", device_path, device_path_instance);
-	return EFI_EXIT(NULL);
+	EFI_ENTRY("%pD, %pD", device_path, device_path_instance);
+	return EFI_EXIT(efi_dp_append_instance(device_path,
+					       device_path_instance));
 }
 
+/*
+ * Get next device path instance.
+ *
+ * This function implements the GetNextDevicePathInstance service of the device
+ * path utilities protocol.
+ *
+ * See the Unified Extensible Firmware Interface (UEFI) specification
+ * for details.
+ *
+ * @device_path_instance	next device path instance
+ * @device_path_instance_size	size of the device path instance
+ * @return			concatenated device path
+ */
 static struct efi_device_path * EFIAPI get_next_device_path_instance(
 	struct efi_device_path **device_path_instance,
 	efi_uintn_t *device_path_instance_size)
 {
-	EFI_ENTRY("%p, %p", device_path_instance, device_path_instance_size);
-	return EFI_EXIT(NULL);
+	EFI_ENTRY("%pD, %p", device_path_instance, device_path_instance_size);
+	return EFI_EXIT(efi_dp_get_next_instance(device_path_instance,
+						 device_path_instance_size));
 }
 
+/*
+ * Check if a device path contains more than one instance.
+ *
+ * This function implements the AppendDeviceNode service of the device path
+ * utilities protocol.
+ *
+ * See the Unified Extensible Firmware Interface (UEFI) specification
+ * for details.
+ *
+ * @device_path		device path
+ * @device_node		device node
+ * @return		concatenated device path
+ */
 static bool EFIAPI is_device_path_multi_instance(
 	const struct efi_device_path *device_path)
 {
-	EFI_ENTRY("%p", device_path);
-	return EFI_EXIT(false);
+	EFI_ENTRY("%pD", device_path);
+	return EFI_EXIT(efi_dp_is_multi_instance(device_path));
 }
 
 /*