[kvm-unit-tests PATCH v5 18/29] lib/efi: Add support for getting the cmdline

[Nikos Nikoleris <nikos.nikoleris@arm.com>]

This change adds support for discovering the command line arguments,
as a string. Then, we parse this string to populate __argc and __argv
for EFI tests.

Signed-off-by: Nikos Nikoleris <nikos.nikoleris@arm.com>
---
 lib/linux/efi.h |  20 ++++++++++
 lib/argv.h      |   1 +
 lib/argv.c      |   2 +-
 lib/efi.c       | 102 ++++++++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 124 insertions(+), 1 deletion(-)

diff --git a/lib/linux/efi.h b/lib/linux/efi.h
index 455625aa..9a1cf87b 100644
--- a/lib/linux/efi.h
+++ b/lib/linux/efi.h
@@ -60,6 +60,8 @@ typedef guid_t efi_guid_t;
 
 #define ACPI_TABLE_GUID EFI_GUID(0xeb9d2d30, 0x2d88, 0x11d3, 0x9a, 0x16, 0x00, 0x90, 0x27, 0x3f, 0xc1, 0x4d)
 
+#define LOADED_IMAGE_PROTOCOL_GUID EFI_GUID(0x5b1b31a1, 0x9562, 0x11d2,  0x8e, 0x3f, 0x00, 0xa0, 0xc9, 0x69, 0x72, 0x3b)
+
 typedef struct {
 	efi_guid_t guid;
 	void *table;
@@ -416,6 +418,24 @@ struct efi_boot_memmap {
 	unsigned long           *buff_size;
 };
 
+#define __aligned_u64 u64 __attribute__((aligned(8)))
+
+struct efi_loaded_image_64 {
+	u32			revision;
+	efi_handle_t		parent_handle;
+	efi_system_table_t	*system_table;
+	efi_handle_t		device_handle;
+	void			*file_path;
+	void			*reserved;
+	u32			load_options_size;
+	void			*load_options;
+	void			*image_base;
+	__aligned_u64		image_size;
+	unsigned int		image_code_type;
+	unsigned int		image_data_type;
+	efi_status_t		(__efiapi * unload)(efi_handle_t image_handle);
+};
+
 #define efi_bs_call(func, ...) efi_system_table->boottime->func(__VA_ARGS__)
 #define efi_rs_call(func, ...) efi_system_table->runtime->func(__VA_ARGS__)
 
diff --git a/lib/argv.h b/lib/argv.h
index 1fd746dc..0fa77725 100644
--- a/lib/argv.h
+++ b/lib/argv.h
@@ -9,6 +9,7 @@
 #define _ARGV_H_
 
 extern void __setup_args(void);
+extern void setup_args(const char *args);
 extern void setup_args_progname(const char *args);
 extern void setup_env(char *env, int size);
 extern void add_setup_arg(const char *arg);
diff --git a/lib/argv.c b/lib/argv.c
index 9ffa673e..fa5ff9ae 100644
--- a/lib/argv.c
+++ b/lib/argv.c
@@ -41,7 +41,7 @@ void __setup_args(void)
 	__argc = argv - __argv;
 }
 
-static void setup_args(const char *args)
+void setup_args(const char *args)
 {
 	if (!args)
 		return;
diff --git a/lib/efi.c b/lib/efi.c
index 64cc9789..f524ec9b 100644
--- a/lib/efi.c
+++ b/lib/efi.c
@@ -8,6 +8,9 @@
  */
 
 #include "efi.h"
+#include <argv.h>
+#include <stdlib.h>
+#include <ctype.h>
 #include <libcflat.h>
 #include <asm/setup.h>
 
@@ -96,6 +99,80 @@ static void efi_exit(efi_status_t code)
 	efi_rs_call(reset_system, EFI_RESET_SHUTDOWN, code, 0, NULL);
 }
 
+/* Adapted from drivers/firmware/efi/libstub/efi-stub.c */
+static char *efi_convert_cmdline(struct efi_loaded_image_64 *image, int *cmd_line_len)
+{
+	const u16 *s2;
+	unsigned long cmdline_addr = 0;
+	int options_chars = image->load_options_size;
+	const u16 *options = image->load_options;
+	int options_bytes = 0, safe_options_bytes = 0;  /* UTF-8 bytes */
+	bool in_quote = false;
+	efi_status_t status;
+	const int COMMAND_LINE_SIZE = 2048;
+
+	if (options) {
+		s2 = options;
+		while (options_bytes < COMMAND_LINE_SIZE && options_chars--) {
+			u16 c = *s2++;
+
+			if (c < 0x80) {
+				if (c == L'\0' || c == L'\n')
+					break;
+				if (c == L'"')
+					in_quote = !in_quote;
+				else if (!in_quote && isspace((char)c))
+					safe_options_bytes = options_bytes;
+
+				options_bytes++;
+				continue;
+			}
+
+			/*
+			 * Get the number of UTF-8 bytes corresponding to a
+			 * UTF-16 character.
+			 * The first part handles everything in the BMP.
+			 */
+			options_bytes += 2 + (c >= 0x800);
+			/*
+			 * Add one more byte for valid surrogate pairs. Invalid
+			 * surrogates will be replaced with 0xfffd and take up
+			 * only 3 bytes.
+			 */
+			if ((c & 0xfc00) == 0xd800) {
+				/*
+				 * If the very last word is a high surrogate,
+				 * we must ignore it since we can't access the
+				 * low surrogate.
+				 */
+				if (!options_chars) {
+					options_bytes -= 3;
+				} else if ((*s2 & 0xfc00) == 0xdc00) {
+					options_bytes++;
+					options_chars--;
+					s2++;
+				}
+			}
+		}
+		if (options_bytes >= COMMAND_LINE_SIZE) {
+			options_bytes = safe_options_bytes;
+			printf("Command line is too long: truncated to %d bytes\n",
+			       options_bytes);
+		}
+	}
+
+	options_bytes++;        /* NUL termination */
+
+	status = efi_bs_call(allocate_pool, EFI_LOADER_DATA, options_bytes, (void **)&cmdline_addr);
+	if (status != EFI_SUCCESS)
+		return NULL;
+
+	snprintf((char *)cmdline_addr, options_bytes, "%.*ls", options_bytes - 1, options);
+
+	*cmd_line_len = options_bytes;
+	return (char *)cmdline_addr;
+}
+
 efi_status_t efi_main(efi_handle_t handle, efi_system_table_t *sys_tab)
 {
 	int ret;
@@ -109,6 +186,31 @@ efi_status_t efi_main(efi_handle_t handle, efi_system_table_t *sys_tab)
 	unsigned long map_size = 0, desc_size = 0, key = 0, buff_size = 0;
 	u32 desc_ver;
 
+	/* Helper variables needed to get the cmdline */
+	struct efi_loaded_image_64 *image;
+	efi_guid_t loaded_image_proto = LOADED_IMAGE_PROTOCOL_GUID;
+	char *cmdline_ptr = NULL;
+	int cmdline_size = 0;
+
+	/*
+	 * Get a handle to the loaded image protocol.  This is used to get
+	 * information about the running image, such as size and the command
+	 * line.
+	 */
+	status = efi_bs_call(handle_protocol, handle, &loaded_image_proto, (void *)&image);
+	if (status != EFI_SUCCESS) {
+		printf("Failed to get loaded image protocol\n");
+		goto efi_main_error;
+	}
+
+	cmdline_ptr = efi_convert_cmdline(image, &cmdline_size);
+	if (!cmdline_ptr) {
+		printf("getting command line via LOADED_IMAGE_PROTOCOL\n");
+		status = EFI_OUT_OF_RESOURCES;
+		goto efi_main_error;
+	}
+	setup_args(cmdline_ptr);
+
 	/* Set up efi_bootinfo */
 	efi_bootinfo.mem_map.map = &map;
 	efi_bootinfo.mem_map.map_size = &map_size;
-- 
2.25.1