1 files changed, 201 insertions, 249 deletions

diff --git a/drivers/firmware/efi/libstub/gop.c b/drivers/firmware/efi/libstub/gop.c
index ea5da307d542..72d74436a7a4 100644
--- a/drivers/firmware/efi/libstub/gop.c
+++ b/drivers/firmware/efi/libstub/gop.c
@@ -12,6 +12,7 @@
 #include <linux/string.h>
 #include <asm/efi.h>
 #include <asm/setup.h>
+#include <video/edid.h>
 
 #include "efistub.h"
 
@@ -133,13 +134,11 @@ void efi_parse_option_graphics(char *option)
 
 static u32 choose_mode_modenum(efi_graphics_output_protocol_t *gop)
 {
-	efi_status_t status;
-
+	efi_graphics_output_mode_info_t *info __free(efi_pool) = NULL;
 	efi_graphics_output_protocol_mode_t *mode;
-	efi_graphics_output_mode_info_t *info;
 	unsigned long info_size;
-
 	u32 max_mode, cur_mode;
+	efi_status_t status;
 	int pf;
 
 	mode = efi_table_attr(gop, mode);
@@ -154,17 +153,13 @@ static u32 choose_mode_modenum(efi_graphics_output_protocol_t *gop)
 		return cur_mode;
 	}
 
-	status = efi_call_proto(gop, query_mode, cmdline.mode,
-				&info_size, &info);
+	status = efi_call_proto(gop, query_mode, cmdline.mode, &info_size, &info);
 	if (status != EFI_SUCCESS) {
 		efi_err("Couldn't get mode information\n");
 		return cur_mode;
 	}
 
 	pf = info->pixel_format;
-
-	efi_bs_call(free_pool, info);
-
 	if (pf == PIXEL_BLT_ONLY || pf >= PIXEL_FORMAT_MAX) {
 		efi_err("Invalid PixelFormat\n");
 		return cur_mode;
@@ -173,6 +168,28 @@ static u32 choose_mode_modenum(efi_graphics_output_protocol_t *gop)
 	return cmdline.mode;
 }
 
+static u32 choose_mode(efi_graphics_output_protocol_t *gop,
+		       bool (*match)(const efi_graphics_output_mode_info_t *, u32, void *),
+		       void *ctx)
+{
+	efi_graphics_output_protocol_mode_t *mode = efi_table_attr(gop, mode);
+	u32 max_mode = efi_table_attr(mode, max_mode);
+
+	for (u32 m = 0; m < max_mode; m++) {
+		efi_graphics_output_mode_info_t *info __free(efi_pool) = NULL;
+		unsigned long info_size;
+		efi_status_t status;
+
+		status = efi_call_proto(gop, query_mode, m, &info_size, &info);
+		if (status != EFI_SUCCESS)
+			continue;
+
+		if (match(info, m, ctx))
+			return m;
+	}
+	return (unsigned long)ctx;
+}
+
 static u8 pixel_bpp(int pixel_format, efi_pixel_bitmask_t pixel_info)
 {
 	if (pixel_format == PIXEL_BIT_MASK) {
@@ -185,192 +202,117 @@ static u8 pixel_bpp(int pixel_format, efi_pixel_bitmask_t pixel_info)
 		return 32;
 }
 
-static u32 choose_mode_res(efi_graphics_output_protocol_t *gop)
+static bool match_res(const efi_graphics_output_mode_info_t *info, u32 mode, void *ctx)
 {
-	efi_status_t status;
+	efi_pixel_bitmask_t pi = info->pixel_information;
+	int pf = info->pixel_format;
 
-	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;
-	efi_pixel_bitmask_t pi;
-	u32 m, w, h;
+	if (pf == PIXEL_BLT_ONLY || pf >= PIXEL_FORMAT_MAX)
+		return false;
 
-	mode = efi_table_attr(gop, mode);
+	return cmdline.res.width == info->horizontal_resolution &&
+	       cmdline.res.height == info->vertical_resolution &&
+	       (cmdline.res.format < 0 || cmdline.res.format == pf) &&
+	       (!cmdline.res.depth || cmdline.res.depth == pixel_bpp(pf, pi));
+}
 
-	cur_mode = efi_table_attr(mode, mode);
-	info = efi_table_attr(mode, info);
-	pf = info->pixel_format;
-	pi = info->pixel_information;
-	w  = info->horizontal_resolution;
-	h  = info->vertical_resolution;
+static u32 choose_mode_res(efi_graphics_output_protocol_t *gop)
+{
+	efi_graphics_output_protocol_mode_t *mode = efi_table_attr(gop, mode);
+	unsigned long cur_mode = efi_table_attr(mode, mode);
 
-	if (w == cmdline.res.width && h == cmdline.res.height &&
-	    (cmdline.res.format < 0 || cmdline.res.format == pf) &&
-	    (!cmdline.res.depth || cmdline.res.depth == pixel_bpp(pf, pi)))
+	if (match_res(efi_table_attr(mode, info), cur_mode, NULL))
 		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;
+	return choose_mode(gop, match_res, (void *)cur_mode);
+}
 
-		pf = info->pixel_format;
-		pi = info->pixel_information;
-		w  = info->horizontal_resolution;
-		h  = info->vertical_resolution;
+struct match {
+	u32	mode;
+	u32	area;
+	u8	depth;
+};
 
-		efi_bs_call(free_pool, info);
+static bool match_auto(const efi_graphics_output_mode_info_t *info, u32 mode, void *ctx)
+{
+	u32 area = info->horizontal_resolution * info->vertical_resolution;
+	efi_pixel_bitmask_t pi = info->pixel_information;
+	int pf = info->pixel_format;
+	u8 depth = pixel_bpp(pf, pi);
+	struct match *m = ctx;
 
-		if (pf == PIXEL_BLT_ONLY || pf >= PIXEL_FORMAT_MAX)
-			continue;
-		if (w == cmdline.res.width && h == cmdline.res.height &&
-		    (cmdline.res.format < 0 || cmdline.res.format == pf) &&
-		    (!cmdline.res.depth || cmdline.res.depth == pixel_bpp(pf, pi)))
-			return m;
-	}
+	if (pf == PIXEL_BLT_ONLY || pf >= PIXEL_FORMAT_MAX)
+		return false;
 
-	efi_err("Couldn't find requested mode\n");
+	if (area > m->area || (area == m->area && depth > m->depth))
+		*m = (struct match){ mode, area, depth };
 
-	return cur_mode;
+	return false;
 }
 
 static u32 choose_mode_auto(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, best_mode, area;
-	u8 depth;
-	int pf;
-	efi_pixel_bitmask_t pi;
-	u32 m, w, h, a;
-	u8 d;
-
-	mode = efi_table_attr(gop, mode);
-
-	cur_mode = efi_table_attr(mode, mode);
-	max_mode = efi_table_attr(mode, max_mode);
-
-	info = efi_table_attr(mode, info);
-
-	pf = info->pixel_format;
-	pi = info->pixel_information;
-	w  = info->horizontal_resolution;
-	h  = info->vertical_resolution;
-
-	best_mode = cur_mode;
-	area = w * h;
-	depth = pixel_bpp(pf, pi);
+	struct match match = {};
 
-	for (m = 0; m < max_mode; m++) {
-		if (m == cur_mode)
-			continue;
+	choose_mode(gop, match_auto, &match);
 
-		status = efi_call_proto(gop, query_mode, m,
-					&info_size, &info);
-		if (status != EFI_SUCCESS)
-			continue;
+	return match.mode;
+}
 
-		pf = info->pixel_format;
-		pi = info->pixel_information;
-		w  = info->horizontal_resolution;
-		h  = info->vertical_resolution;
+static bool match_list(const efi_graphics_output_mode_info_t *info, u32 mode, void *ctx)
+{
+	efi_pixel_bitmask_t pi = info->pixel_information;
+	u32 cur_mode = (unsigned long)ctx;
+	int pf = info->pixel_format;
+	const char *dstr;
+	u8 depth = 0;
+	bool valid;
 
-		efi_bs_call(free_pool, info);
+	valid = !(pf == PIXEL_BLT_ONLY || pf >= PIXEL_FORMAT_MAX);
 
-		if (pf == PIXEL_BLT_ONLY || pf >= PIXEL_FORMAT_MAX)
-			continue;
-		a = w * h;
-		if (a < area)
-			continue;
-		d = pixel_bpp(pf, pi);
-		if (a > area || d > depth) {
-			best_mode = m;
-			area = a;
-			depth = d;
-		}
+	switch (pf) {
+	case PIXEL_RGB_RESERVED_8BIT_PER_COLOR:
+		dstr = "rgb";
+		break;
+	case PIXEL_BGR_RESERVED_8BIT_PER_COLOR:
+		dstr = "bgr";
+		break;
+	case PIXEL_BIT_MASK:
+		dstr = "";
+		depth = pixel_bpp(pf, pi);
+		break;
+	case PIXEL_BLT_ONLY:
+		dstr = "blt";
+		break;
+	default:
+		dstr = "xxx";
+		break;
 	}
 
-	return best_mode;
+	efi_printk("Mode %3u %c%c: Resolution %ux%u-%s%.0hhu\n",
+		    mode,
+		    (mode == cur_mode) ? '*' : ' ',
+		    !valid ? '-' : ' ',
+		    info->horizontal_resolution,
+		    info->vertical_resolution,
+		    dstr, depth);
+
+	return false;
 }
 
 static u32 choose_mode_list(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;
-	efi_pixel_bitmask_t pi;
-	u32 m, w, h;
-	u8 d;
-	const char *dstr;
-	bool valid;
+	efi_graphics_output_protocol_mode_t *mode = efi_table_attr(gop, mode);
+	unsigned long cur_mode = efi_table_attr(mode, mode);
+	u32 max_mode = efi_table_attr(mode, max_mode);
 	efi_input_key_t key;
-
-	mode = efi_table_attr(gop, mode);
-
-	cur_mode = efi_table_attr(mode, mode);
-	max_mode = efi_table_attr(mode, max_mode);
+	efi_status_t status;
 
 	efi_printk("Available graphics modes are 0-%u\n", max_mode-1);
 	efi_puts("  * = current mode\n"
 		 "  - = unusable mode\n");
-	for (m = 0; m < max_mode; m++) {
-		status = efi_call_proto(gop, query_mode, m,
-					&info_size, &info);
-		if (status != EFI_SUCCESS)
-			continue;
 
-		pf = info->pixel_format;
-		pi = info->pixel_information;
-		w  = info->horizontal_resolution;
-		h  = info->vertical_resolution;
-
-		efi_bs_call(free_pool, info);
-
-		valid = !(pf == PIXEL_BLT_ONLY || pf >= PIXEL_FORMAT_MAX);
-		d = 0;
-		switch (pf) {
-		case PIXEL_RGB_RESERVED_8BIT_PER_COLOR:
-			dstr = "rgb";
-			break;
-		case PIXEL_BGR_RESERVED_8BIT_PER_COLOR:
-			dstr = "bgr";
-			break;
-		case PIXEL_BIT_MASK:
-			dstr = "";
-			d = pixel_bpp(pf, pi);
-			break;
-		case PIXEL_BLT_ONLY:
-			dstr = "blt";
-			break;
-		default:
-			dstr = "xxx";
-			break;
-		}
-
-		efi_printk("Mode %3u %c%c: Resolution %ux%u-%s%.0hhu\n",
-			   m,
-			   m == cur_mode ? '*' : ' ',
-			   !valid ? '-' : ' ',
-			   w, h, dstr, d);
-	}
+	choose_mode(gop, match_list, (void *)cur_mode);
 
 	efi_puts("\nPress any key to continue (or wait 10 seconds)\n");
 	status = efi_wait_for_key(10 * EFI_USEC_PER_SEC, &key);
@@ -426,24 +368,31 @@ static void find_bits(u32 mask, u8 *pos, u8 *size)
 	*size = __fls(mask) - *pos + 1;
 }
 
-static void
-setup_pixel_info(struct screen_info *si, u32 pixels_per_scan_line,
-		 efi_pixel_bitmask_t pixel_info, int pixel_format)
+static void setup_screen_info(struct screen_info *si, const efi_graphics_output_protocol_t *gop)
 {
-	if (pixel_format == PIXEL_BIT_MASK) {
-		find_bits(pixel_info.red_mask,
-			  &si->red_pos, &si->red_size);
-		find_bits(pixel_info.green_mask,
-			  &si->green_pos, &si->green_size);
-		find_bits(pixel_info.blue_mask,
-			  &si->blue_pos, &si->blue_size);
-		find_bits(pixel_info.reserved_mask,
-			  &si->rsvd_pos, &si->rsvd_size);
-		si->lfb_depth = si->red_size + si->green_size +
-			si->blue_size + si->rsvd_size;
-		si->lfb_linelength = (pixels_per_scan_line * si->lfb_depth) / 8;
+	const efi_graphics_output_protocol_mode_t *mode = efi_table_attr(gop, mode);
+	const efi_graphics_output_mode_info_t *info = efi_table_attr(mode, info);
+
+	si->orig_video_isVGA = VIDEO_TYPE_EFI;
+
+	si->lfb_width  = info->horizontal_resolution;
+	si->lfb_height = info->vertical_resolution;
+
+	efi_set_u64_split(efi_table_attr(mode, frame_buffer_base),
+			  &si->lfb_base, &si->ext_lfb_base);
+	if (si->ext_lfb_base)
+		si->capabilities |= VIDEO_CAPABILITY_64BIT_BASE;
+	si->pages = 1;
+
+	if (info->pixel_format == PIXEL_BIT_MASK) {
+		find_bits(info->pixel_information.red_mask, &si->red_pos, &si->red_size);
+		find_bits(info->pixel_information.green_mask, &si->green_pos, &si->green_size);
+		find_bits(info->pixel_information.blue_mask, &si->blue_pos, &si->blue_size);
+		find_bits(info->pixel_information.reserved_mask, &si->rsvd_pos, &si->rsvd_size);
+		si->lfb_depth = si->red_size + si->green_size + si->blue_size + si->rsvd_size;
+		si->lfb_linelength = (info->pixels_per_scan_line * si->lfb_depth) / 8;
 	} else {
-		if (pixel_format == PIXEL_RGB_RESERVED_8BIT_PER_COLOR) {
+		if (info->pixel_format == PIXEL_RGB_RESERVED_8BIT_PER_COLOR) {
 			si->red_pos   = 0;
 			si->blue_pos  = 16;
 		} else /* PIXEL_BGR_RESERVED_8BIT_PER_COLOR */ {
@@ -453,34 +402,46 @@ setup_pixel_info(struct screen_info *si, u32 pixels_per_scan_line,
 
 		si->green_pos = 8;
 		si->rsvd_pos  = 24;
-		si->red_size = si->green_size =
-			si->blue_size = si->rsvd_size = 8;
-
+		si->red_size = 8;
+		si->green_size = 8;
+		si->blue_size = 8;
+		si->rsvd_size = 8;
 		si->lfb_depth = 32;
-		si->lfb_linelength = pixels_per_scan_line * 4;
+		si->lfb_linelength = info->pixels_per_scan_line * 4;
 	}
+
+	si->lfb_size = si->lfb_linelength * si->lfb_height;
+	si->capabilities |= VIDEO_CAPABILITY_SKIP_QUIRKS;
 }
 
-static efi_graphics_output_protocol_t *
-find_gop(efi_guid_t *proto, unsigned long size, void **handles)
+static void setup_edid_info(struct edid_info *edid, u32 gop_size_of_edid, u8 *gop_edid)
+{
+	if (!gop_edid || gop_size_of_edid < 128)
+		memset(edid->dummy, 0, sizeof(edid->dummy));
+	else
+		memcpy(edid->dummy, gop_edid, min(gop_size_of_edid, sizeof(edid->dummy)));
+}
+
+static efi_handle_t find_handle_with_primary_gop(unsigned long num, const efi_handle_t handles[],
+						 efi_graphics_output_protocol_t **found_gop)
 {
 	efi_graphics_output_protocol_t *first_gop;
-	efi_handle_t h;
-	int i;
+	efi_handle_t h, first_gop_handle;
 
+	first_gop_handle = NULL;
 	first_gop = NULL;
 
-	for_each_efi_handle(h, handles, size, i) {
+	for_each_efi_handle(h, handles, num) {
 		efi_status_t status;
 
 		efi_graphics_output_protocol_t *gop;
 		efi_graphics_output_protocol_mode_t *mode;
 		efi_graphics_output_mode_info_t *info;
-
-		efi_guid_t conout_proto = EFI_CONSOLE_OUT_DEVICE_GUID;
 		void *dummy = NULL;
 
-		status = efi_bs_call(handle_protocol, h, proto, (void **)&gop);
+		status = efi_bs_call(handle_protocol, h,
+				     &EFI_GRAPHICS_OUTPUT_PROTOCOL_GUID,
+				     (void **)&gop);
 		if (status != EFI_SUCCESS)
 			continue;
 
@@ -500,81 +461,72 @@ find_gop(efi_guid_t *proto, unsigned long size, void **handles)
 		 * Once we've found a GOP supporting ConOut,
 		 * don't bother looking any further.
 		 */
-		status = efi_bs_call(handle_protocol, h, &conout_proto, &dummy);
-		if (status == EFI_SUCCESS)
-			return gop;
-
-		if (!first_gop)
+		status = efi_bs_call(handle_protocol, h,
+				     &EFI_CONSOLE_OUT_DEVICE_GUID, &dummy);
+		if (status == EFI_SUCCESS) {
+			if (found_gop)
+				*found_gop = gop;
+			return h;
+		} else if (!first_gop_handle) {
+			first_gop_handle = h;
 			first_gop = gop;
+		}
 	}
 
-	return first_gop;
+	if (found_gop)
+		*found_gop = first_gop;
+	return first_gop_handle;
 }
 
-static efi_status_t setup_gop(struct screen_info *si, efi_guid_t *proto,
-			      unsigned long size, void **handles)
+efi_status_t efi_setup_graphics(struct screen_info *si, struct edid_info *edid)
 {
+	efi_handle_t *handles __free(efi_pool) = NULL;
+	efi_handle_t handle;
 	efi_graphics_output_protocol_t *gop;
-	efi_graphics_output_protocol_mode_t *mode;
-	efi_graphics_output_mode_info_t *info;
+	efi_status_t status;
+	unsigned long num;
 
-	gop = find_gop(proto, size, handles);
+	status = efi_bs_call(locate_handle_buffer, EFI_LOCATE_BY_PROTOCOL,
+			      &EFI_GRAPHICS_OUTPUT_PROTOCOL_GUID, NULL, &num,
+			      &handles);
+	if (status != EFI_SUCCESS)
+		return status;
 
-	/* Did we find any GOPs? */
-	if (!gop)
+	handle = find_handle_with_primary_gop(num, handles, &gop);
+	if (!handle)
 		return EFI_NOT_FOUND;
 
 	/* Change mode if requested */
 	set_mode(gop);
 
 	/* EFI framebuffer */
-	mode = efi_table_attr(gop, mode);
-	info = efi_table_attr(mode, info);
-
-	si->orig_video_isVGA = VIDEO_TYPE_EFI;
-
-	si->lfb_width  = info->horizontal_resolution;
-	si->lfb_height = info->vertical_resolution;
-
-	efi_set_u64_split(efi_table_attr(mode, frame_buffer_base),
-			  &si->lfb_base, &si->ext_lfb_base);
-	if (si->ext_lfb_base)
-		si->capabilities |= VIDEO_CAPABILITY_64BIT_BASE;
-
-	si->pages = 1;
-
-	setup_pixel_info(si, info->pixels_per_scan_line,
-			     info->pixel_information, info->pixel_format);
-
-	si->lfb_size = si->lfb_linelength * si->lfb_height;
+	if (si)
+		setup_screen_info(si, gop);
+
+	/* Display EDID for primary GOP */
+	if (edid) {
+		efi_edid_discovered_protocol_t *discovered_edid;
+		efi_edid_active_protocol_t *active_edid;
+		u32 gop_size_of_edid = 0;
+		u8 *gop_edid = NULL;
+
+		status = efi_bs_call(handle_protocol, handle, &EFI_EDID_ACTIVE_PROTOCOL_GUID,
+				     (void **)&active_edid);
+		if (status == EFI_SUCCESS) {
+			gop_size_of_edid = active_edid->size_of_edid;
+			gop_edid = active_edid->edid;
+		} else {
+			status = efi_bs_call(handle_protocol, handle,
+					     &EFI_EDID_DISCOVERED_PROTOCOL_GUID,
+					     (void **)&discovered_edid);
+			if (status == EFI_SUCCESS) {
+				gop_size_of_edid = discovered_edid->size_of_edid;
+				gop_edid = discovered_edid->edid;
+			}
+		}
 
-	si->capabilities |= VIDEO_CAPABILITY_SKIP_QUIRKS;
+		setup_edid_info(edid, gop_size_of_edid, gop_edid);
+	}
 
 	return EFI_SUCCESS;
 }
-
-/*
- * See if we have Graphics Output Protocol
- */
-efi_status_t efi_setup_gop(struct screen_info *si, efi_guid_t *proto,
-			   unsigned long size)
-{
-	efi_status_t status;
-	void **gop_handle = NULL;
-
-	status = efi_bs_call(allocate_pool, EFI_LOADER_DATA, size,
-			     (void **)&gop_handle);
-	if (status != EFI_SUCCESS)
-		return status;
-
-	status = efi_bs_call(locate_handle, EFI_LOCATE_BY_PROTOCOL, proto, NULL,
-			     &size, gop_handle);
-	if (status != EFI_SUCCESS)
-		goto free_handle;
-
-	status = setup_gop(si, proto, size, gop_handle);
-
-free_handle:
-	efi_bs_call(free_pool, gop_handle);
-	return status;
-}