efi_loader: implement CreateDeviceNode

Implement the CreateDeviceNode service of the device path utility protocol.

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

3 files changed, 35 insertions, 2 deletions

diff --git a/include/efi_loader.h b/include/efi_loader.h
index 17f9d3d1ef..0358bcb1d7 100644
--- a/include/efi_loader.h
+++ b/include/efi_loader.h
@@ -330,7 +330,10 @@ struct efi_device_path *efi_dp_append(const struct efi_device_path *dp1,
 				      const struct efi_device_path *dp2);
 struct efi_device_path *efi_dp_append_node(const struct efi_device_path *dp,
 					   const struct efi_device_path *node);
-
+/* Create a device path node of given type, sub-type, length */
+struct efi_device_path *efi_dp_create_device_node(const u8 type,
+						  const u8 sub_type,
+						  const u16 length);
 
 struct efi_device_path *efi_dp_from_dev(struct udevice *dev);
 struct efi_device_path *efi_dp_from_part(struct blk_desc *desc, int part);
diff --git a/lib/efi_loader/efi_device_path.c b/lib/efi_loader/efi_device_path.c
index e965f1d88e..0cd0b459e3 100644
--- a/lib/efi_loader/efi_device_path.c
+++ b/lib/efi_loader/efi_device_path.c
@@ -315,6 +315,21 @@ struct efi_device_path *efi_dp_append_node(const struct efi_device_path *dp,
 	return ret;
 }
 
+struct efi_device_path *efi_dp_create_device_node(const u8 type,
+						  const u8 sub_type,
+						  const u16 length)
+{
+	struct efi_device_path *ret;
+
+	ret = dp_alloc(length);
+	if (!ret)
+		return ret;
+	ret->type = type;
+	ret->sub_type = sub_type;
+	ret->length = length;
+	return ret;
+}
+
 #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 bc97eeee31..e73188b242 100644
--- a/lib/efi_loader/efi_device_path_utilities.c
+++ b/lib/efi_loader/efi_device_path_utilities.c
@@ -70,11 +70,26 @@ static bool EFIAPI is_device_path_multi_instance(
 	return EFI_EXIT(false);
 }
 
+/*
+ * Create device node.
+ *
+ * This function implements the CreateDeviceNode service of the device path
+ * utilities protocol.
+ *
+ * See the Unified Extensible Firmware Interface (UEFI) specification
+ * for details.
+ *
+ * @node_type		node type
+ * @node_sub_type	node sub type
+ * @node_length		node length
+ * @return		device path node
+ */
 static struct efi_device_path * EFIAPI create_device_node(
 	uint8_t node_type, uint8_t node_sub_type, uint16_t node_length)
 {
 	EFI_ENTRY("%u, %u, %u", node_type, node_sub_type, node_length);
-	return EFI_EXIT(NULL);
+	return EFI_EXIT(efi_dp_create_device_node(node_type, node_sub_type,
+			node_length));
 }
 
 const struct efi_device_path_utilities_protocol efi_device_path_utilities = {