[PATCH 2/4] efi/libstub: detect panel-id

[Rob Clark <robdclark@gmail.com>]

From: Rob Clark <robdclark@chromium.org>

On snapdragon aarch64 laptops, a 'UEFIDisplayInfo' variable is provided
to communicate some information about the display.  Crutially it has the
panel-id, so the appropriate panel driver can be selected.  Read this
out and stash in /chosen/panel-id so that display driver can use it to
pick the appropriate panel.

Signed-off-by: Rob Clark <robdclark@chromium.org>
---
 drivers/firmware/efi/libstub/arm-stub.c | 49 +++++++++++++++++++++++++
 drivers/firmware/efi/libstub/efistub.h  |  2 +
 drivers/firmware/efi/libstub/fdt.c      |  9 +++++
 3 files changed, 60 insertions(+)

diff --git a/drivers/firmware/efi/libstub/arm-stub.c b/drivers/firmware/efi/libstub/arm-stub.c
index 04e6ecd72cd9..999813252e0d 100644
--- a/drivers/firmware/efi/libstub/arm-stub.c
+++ b/drivers/firmware/efi/libstub/arm-stub.c
@@ -69,6 +69,53 @@ static struct screen_info *setup_graphics(efi_system_table_t *sys_table_arg)
 	return si;
 }
 
+/*
+ * We (at least currently) don't care about most of the fields, just
+ * panel_id:
+ */
+struct mdp_disp_info {
+	u32 version_info;
+	u32 pad0[9];
+	u32 panel_id;
+	u32 pad1[17];
+};
+
+#define MDP_DISP_INFO_VERSION_MAGIC 0xaa
+
+static void get_panel_id(efi_system_table_t *sys_table_arg,
+			 unsigned long fdt_addr)
+{
+	efi_guid_t gop_proto = EFI_GRAPHICS_OUTPUT_PROTOCOL_GUID;
+	efi_status_t status;
+	struct mdp_disp_info *disp_info;
+	unsigned long size = 0;
+
+	status = efi_call_runtime(get_variable, L"UEFIDisplayInfo",
+				  &gop_proto, NULL, &size, NULL);
+	if (status == EFI_NOT_FOUND)
+		return;
+
+	status = efi_call_early(allocate_pool, EFI_LOADER_DATA, size,
+				(void **)&disp_info);
+	if (status != EFI_SUCCESS)
+		return;
+
+	status = efi_call_runtime(get_variable, L"UEFIDisplayInfo",
+				  &gop_proto, NULL, &size, disp_info);
+	if (status != EFI_SUCCESS)
+		goto cleanup;
+
+	if ((disp_info->version_info >> 16) != MDP_DISP_INFO_VERSION_MAGIC)
+		goto cleanup;
+
+	efi_printk(sys_table_arg, "found a panel-id!\n");
+
+	set_chosen_panel_id(fdt_addr, disp_info->panel_id);
+
+cleanup:
+	efi_call_early(free_pool, disp_info);
+}
+
 void install_memreserve_table(efi_system_table_t *sys_table_arg)
 {
 	struct linux_efi_memreserve *rsv;
@@ -229,6 +276,8 @@ unsigned long efi_entry(void *handle, efi_system_table_t *sys_table,
 	if (!fdt_addr)
 		pr_efi(sys_table, "Generating empty DTB\n");
 
+	get_panel_id(sys_table, fdt_addr);
+
 	status = handle_cmdline_files(sys_table, image, cmdline_ptr, "initrd=",
 				      efi_get_max_initrd_addr(dram_base,
 							      *image_addr),
diff --git a/drivers/firmware/efi/libstub/efistub.h b/drivers/firmware/efi/libstub/efistub.h
index 1b1dfcaa6fb9..8832cb9a7a40 100644
--- a/drivers/firmware/efi/libstub/efistub.h
+++ b/drivers/firmware/efi/libstub/efistub.h
@@ -39,6 +39,8 @@ void efi_char16_printk(efi_system_table_t *, efi_char16_t *);
 
 unsigned long get_dram_base(efi_system_table_t *sys_table_arg);
 
+void set_chosen_panel_id(unsigned long fdt_addr, unsigned panel_id);
+
 efi_status_t allocate_new_fdt_and_exit_boot(efi_system_table_t *sys_table,
 					    void *handle,
 					    unsigned long *new_fdt_addr,
diff --git a/drivers/firmware/efi/libstub/fdt.c b/drivers/firmware/efi/libstub/fdt.c
index 5440ba17a1c5..cb6ea160a40a 100644
--- a/drivers/firmware/efi/libstub/fdt.c
+++ b/drivers/firmware/efi/libstub/fdt.c
@@ -200,6 +200,15 @@ static efi_status_t update_fdt_memmap(void *fdt, struct efi_boot_memmap *map)
 	return EFI_SUCCESS;
 }
 
+void set_chosen_panel_id(unsigned long fdt_addr, unsigned panel_id)
+{
+	void *fdt = (void *)fdt_addr;
+	int node = fdt_subnode_offset(fdt, 0, "chosen");
+	u32 fdt_val32 = cpu_to_fdt32(panel_id);
+
+	fdt_setprop_var(fdt, node, "panel-id", fdt_val32);
+}
+
 #ifndef EFI_FDT_ALIGN
 # define EFI_FDT_ALIGN EFI_PAGE_SIZE
 #endif
-- 
2.20.1