[PATCH v2 12/14] efi/gop: Allow specifying mode by <xres>x<yres>

[Arvind Sankar <nivedita@xxxxxxxxxxxx>]

Add the ability to choose a video mode using a command-line argument of
the form
	video=efifb:<xres>x<yres>

Signed-off-by: Arvind Sankar <nivedita@xxxxxxxxxxxx>
---
 Documentation/fb/efifb.rst         |  5 ++
 drivers/firmware/efi/libstub/gop.c | 84 +++++++++++++++++++++++++++++-
 2 files changed, 88 insertions(+), 1 deletion(-)

diff --git a/Documentation/fb/efifb.rst b/Documentation/fb/efifb.rst
index 367fbda2f4da..635275071307 100644
--- a/Documentation/fb/efifb.rst
+++ b/Documentation/fb/efifb.rst
@@ -50,4 +50,9 @@ mode=n
         The EFI stub will set the mode of the display to mode number n if
         possible.
 
+<xres>x<yres>
+        The EFI stub will search for a display mode that matches the specified
+        horizontal and vertical resolution, and set the mode of the display to
+        it if one is found.
+
 Edgar Hucek <gimli@xxxxxxxxxxxxxx>
diff --git a/drivers/firmware/efi/libstub/gop.c b/drivers/firmware/efi/libstub/gop.c
index a32b784b4577..cc84e6a82f54 100644
--- a/drivers/firmware/efi/libstub/gop.c
+++ b/drivers/firmware/efi/libstub/gop.c
@@ -6,6 +6,7 @@
  * ----------------------------------------------------------------------- */
 
 #include <linux/bitops.h>
+#include <linux/ctype.h>
 #include <linux/efi.h>
 #include <linux/screen_info.h>
 #include <linux/string.h>
@@ -17,11 +18,17 @@
 enum efi_cmdline_option {
 	EFI_CMDLINE_NONE,
 	EFI_CMDLINE_MODE_NUM,
+	EFI_CMDLINE_RES
 };
 
 static struct {
 	enum efi_cmdline_option option;
-	u32 mode;
+	union {
+		u32 mode;
+		struct {
+			u32 width, height;
+		} res;
+	};
 } cmdline __efistub_global = { .option = EFI_CMDLINE_NONE };
 
 static bool parse_modenum(char *option, char **next)
@@ -41,11 +48,33 @@ static bool parse_modenum(char *option, char **next)
 	return true;
 }
 
+static bool parse_res(char *option, char **next)
+{
+	u32 w, h;
+
+	if (!isdigit(*option))
+		return false;
+	w = simple_strtoull(option, &option, 10);
+	if (*option++ != 'x' || !isdigit(*option))
+		return false;
+	h = simple_strtoull(option, &option, 10);
+	if (*option && *option++ != ',')
+		return false;
+	cmdline.option     = EFI_CMDLINE_RES;
+	cmdline.res.width  = w;
+	cmdline.res.height = h;
+
+	*next = option;
+	return true;
+}
+
 void efi_parse_option_graphics(char *option)
 {
 	while (*option) {
 		if (parse_modenum(option, &option))
 			continue;
+		if (parse_res(option, &option))
+			continue;
 
 		while (*option && *option++ != ',')
 			;
@@ -94,6 +123,56 @@ static u32 choose_mode_modenum(efi_graphics_output_protocol_t *gop)
 	return cmdline.mode;
 }
 
+static u32 choose_mode_res(efi_graphics_output_protocol_t *gop)
+{
+	efi_status_t status;
+
+	efi_graphics_output_protocol_mode_t *mode;
+	efi_graphics_output_mode_info_t *info;
+	unsigned long info_size;
+
+	u32 max_mode, cur_mode;
+	int pf;
+	u32 m, w, h;
+
+	mode = efi_table_attr(gop, mode);
+
+	cur_mode = efi_table_attr(mode, mode);
+	info = efi_table_attr(mode, info);
+	w = info->horizontal_resolution;
+	h = info->vertical_resolution;
+
+	if (w == cmdline.res.width && h == cmdline.res.height)
+		return cur_mode;
+
+	max_mode = efi_table_attr(mode, max_mode);
+
+	for (m = 0; m < max_mode; m++) {
+		if (m == cur_mode)
+			continue;
+
+		status = efi_call_proto(gop, query_mode, m,
+					&info_size, &info);
+		if (status != EFI_SUCCESS)
+			continue;
+
+		pf = info->pixel_format;
+		w  = info->horizontal_resolution;
+		h  = info->vertical_resolution;
+
+		efi_bs_call(free_pool, info);
+
+		if (pf == PIXEL_BLT_ONLY || pf >= PIXEL_FORMAT_MAX)
+			continue;
+		if (w == cmdline.res.width && h == cmdline.res.height)
+			return m;
+	}
+
+	efi_printk("Couldn't find requested mode\n");
+
+	return cur_mode;
+}
+
 static void set_mode(efi_graphics_output_protocol_t *gop)
 {
 	efi_graphics_output_protocol_mode_t *mode;
@@ -103,6 +182,9 @@ static void set_mode(efi_graphics_output_protocol_t *gop)
 	case EFI_CMDLINE_MODE_NUM:
 		new_mode = choose_mode_modenum(gop);
 		break;
+	case EFI_CMDLINE_RES:
+		new_mode = choose_mode_res(gop);
+		break;
 	default:
 		return;
 	}
-- 
2.24.1