Merge branch 'for-next/efi'

21 files changed, 1471 insertions, 6 deletions

diff --git a/drivers/Kconfig b/drivers/Kconfig
index cc086ac2df..bf31115fa0 100644
--- a/drivers/Kconfig
+++ b/drivers/Kconfig
@@ -1,5 +1,6 @@
 menu "Drivers"
 
+source "drivers/efi/Kconfig"
 source "drivers/of/Kconfig"
 source "drivers/aiodev/Kconfig"
 source "drivers/amba/Kconfig"
diff --git a/drivers/Makefile b/drivers/Makefile
index 6a70f6ee19..0fadf4e44b 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -1,5 +1,7 @@
 obj-y	+= base/
+obj-y	+= block/
 obj-$(CONFIG_ARM_AMBA) += amba/
+obj-$(CONFIG_EFI_BOOTUP) += efi/
 obj-y	+= net/
 obj-y	+= serial/
 obj-y	+= mtd/
diff --git a/drivers/block/Makefile b/drivers/block/Makefile
new file mode 100644
index 0000000000..8812c0faec
--- /dev/null
+++ b/drivers/block/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_EFI_BOOTUP) += efi-block-io.o
diff --git a/drivers/block/efi-block-io.c b/drivers/block/efi-block-io.c
new file mode 100644
index 0000000000..a4d9d3a95d
--- /dev/null
+++ b/drivers/block/efi-block-io.c
@@ -0,0 +1,174 @@
+#include <common.h>
+#include <driver.h>
+#include <init.h>
+#include <malloc.h>
+#include <fs.h>
+#include <string.h>
+#include <command.h>
+#include <errno.h>
+#include <linux/stat.h>
+#include <xfuncs.h>
+#include <fcntl.h>
+#include <efi.h>
+#include <block.h>
+#include <disks.h>
+#include <efi/efi.h>
+#include <efi/efi-device.h>
+
+#define EFI_BLOCK_IO_PROTOCOL_REVISION2 0x00020001
+#define EFI_BLOCK_IO_PROTOCOL_REVISION3 ((2<<16) | (31))
+
+struct efi_block_io_media{
+	u32 media_id;
+	bool removable_media;
+	bool media_present;
+	bool logical_partition;
+	bool read_only;
+	bool write_caching;
+	u32 block_size;
+	u32 io_align;
+	u64 last_block;
+	u64 lowest_aligned_lba; /* added in Revision 2 */
+	u32 logical_blocks_per_physical_block; /* added in Revision 2 */
+	u32 optimal_transfer_length_granularity; /* added in Revision 3 */
+};
+
+struct efi_block_io_protocol {
+	u64 revision;
+	struct efi_block_io_media *media;
+	efi_status_t(EFIAPI *reset)(struct efi_block_io_protocol *this,
+			bool ExtendedVerification);
+	efi_status_t(EFIAPI *read)(struct efi_block_io_protocol *this, u32 media_id,
+			u64 lba, unsigned long buffer_size, void *buf);
+	efi_status_t(EFIAPI *write)(struct efi_block_io_protocol *this, u32 media_id,
+			u64 lba, unsigned long buffer_size, void *buf);
+	efi_status_t(EFIAPI *flush)(struct efi_block_io_protocol *this);
+};
+
+struct efi_bio_priv {
+	struct efi_block_io_protocol *protocol;
+	struct device_d *dev;
+	struct block_device blk;
+	u32 media_id;
+};
+
+static int efi_bio_read(struct block_device *blk, void *buffer, int block,
+		int num_blocks)
+{
+	struct efi_bio_priv *priv = container_of(blk, struct efi_bio_priv, blk);
+	efi_status_t efiret;
+
+	efiret = priv->protocol->read(priv->protocol, priv->media_id,
+			block, num_blocks * 512, buffer);
+
+	if (EFI_ERROR(efiret))
+		return -efi_errno(efiret);
+
+	return 0;
+}
+
+static int efi_bio_write(struct block_device *blk,
+		const void *buffer, int block, int num_blocks)
+{
+	struct efi_bio_priv *priv = container_of(blk, struct efi_bio_priv, blk);
+	efi_status_t efiret;
+
+	efiret = priv->protocol->write(priv->protocol, priv->media_id,
+			block, num_blocks * 512, (void *)buffer);
+	if (EFI_ERROR(efiret))
+		return -efi_errno(efiret);
+
+	return 0;
+}
+
+static int efi_bio_flush(struct block_device *blk)
+{
+	struct efi_bio_priv *priv = container_of(blk, struct efi_bio_priv, blk);
+	efi_status_t efiret;
+
+	efiret = priv->protocol->flush(priv->protocol);
+	if (EFI_ERROR(efiret))
+		return -efi_errno(efiret);
+
+	return 0;
+}
+
+static struct block_device_ops efi_bio_ops = {
+	.read = efi_bio_read,
+	.write = efi_bio_write,
+	.flush = efi_bio_flush,
+};
+
+static void efi_bio_print_info(struct efi_bio_priv *priv)
+{
+	struct efi_block_io_media *media = priv->protocol->media;
+	u64 revision = priv->protocol->revision;
+
+	dev_dbg(priv->dev, "revision: 0x%016llx\n", revision);
+	dev_dbg(priv->dev, "media_id: 0x%08x\n", media->media_id);
+	dev_dbg(priv->dev, "removable_media: %d\n", media->removable_media);
+	dev_dbg(priv->dev, "media_present: %d\n", media->media_present);
+	dev_dbg(priv->dev, "logical_partition: %d\n", media->logical_partition);
+	dev_dbg(priv->dev, "read_only: %d\n", media->read_only);
+	dev_dbg(priv->dev, "write_caching: %d\n", media->write_caching);
+	dev_dbg(priv->dev, "block_size: 0x%08x\n", media->block_size);
+	dev_dbg(priv->dev, "io_align: 0x%08x\n", media->io_align);
+	dev_dbg(priv->dev, "last_block: 0x%016llx\n", media->last_block);
+
+	if (revision < EFI_BLOCK_IO_PROTOCOL_REVISION2)
+		return;
+
+	dev_dbg(priv->dev, "u64 lowest_aligned_lba: 0x%08llx\n",
+			media->lowest_aligned_lba);
+	dev_dbg(priv->dev, "logical_blocks_per_physical_block: 0x%08x\n",
+			media->logical_blocks_per_physical_block);
+
+	if (revision < EFI_BLOCK_IO_PROTOCOL_REVISION3)
+		return;
+
+	dev_dbg(priv->dev, "optimal_transfer_length_granularity: 0x%08x\n",
+			media->optimal_transfer_length_granularity);
+}
+
+int efi_bio_probe(struct efi_device *efidev)
+{
+	int ret;
+	struct efi_bio_priv *priv;
+	struct efi_block_io_media *media;
+
+	priv = xzalloc(sizeof(*priv));
+
+	BS->handle_protocol(efidev->handle, &efi_block_io_protocol_guid,
+			(void **)&priv->protocol);
+	if (!priv->protocol)
+		return -ENODEV;
+
+	media = priv->protocol->media;
+	efi_bio_print_info(priv);
+	priv->dev = &efidev->dev;
+
+	priv->blk.cdev.name = xasprintf("disk%d", cdev_find_free_index("disk"));
+	priv->blk.blockbits = ffs(media->block_size) - 1;
+	priv->blk.num_blocks = media->last_block + 1;
+	priv->blk.ops = &efi_bio_ops;
+	priv->blk.dev = &efidev->dev;
+
+	priv->media_id = media->media_id;
+
+	ret = blockdevice_register(&priv->blk);
+	if (ret)
+		return ret;
+
+	parse_partition_table(&priv->blk);
+
+	return 0;
+}
+
+static struct efi_driver efi_fs_driver = {
+        .driver = {
+		.name  = "efi-block-io",
+	},
+        .probe = efi_bio_probe,
+	.guid = EFI_BLOCK_IO_PROTOCOL_GUID,
+};
+device_efi_driver(efi_fs_driver);
diff --git a/drivers/clocksource/Kconfig b/drivers/clocksource/Kconfig
index f3c3255ffc..23ad20afcf 100644
--- a/drivers/clocksource/Kconfig
+++ b/drivers/clocksource/Kconfig
@@ -34,6 +34,14 @@ config CLOCKSOURCE_DUMMY_RATE
 	  The option CONFIG_CLOCKSOURCE_DUMMY_RATE is used to adjust this clocksource.
 	  The bigger rate valuest makes clocksource "faster".
 
+config CLOCKSOURCE_EFI
+	bool "Generic EFI Driver"
+	depends on EFI_BOOTUP
+
+config CLOCKSOURCE_EFI_X86
+	bool "EFI X86 HW driver"
+	depends on EFI_BOOTUP && X86
+
 config CLOCKSOURCE_MVEBU
 	bool
 	depends on ARCH_MVEBU
diff --git a/drivers/clocksource/Makefile b/drivers/clocksource/Makefile
index 0564d8f5a9..f774edee46 100644
--- a/drivers/clocksource/Makefile
+++ b/drivers/clocksource/Makefile
@@ -3,6 +3,8 @@ obj-$(CONFIG_ARM_SMP_TWD) += arm_smp_twd.o
 obj-$(CONFIG_CLOCKSOURCE_BCM283X) += bcm2835.o
 obj-$(CONFIG_CLOCKSOURCE_CLPS711X) += clps711x.o
 obj-$(CONFIG_CLOCKSOURCE_DIGIC)   += digic.o
+obj-$(CONFIG_CLOCKSOURCE_EFI) += efi.o
+obj-$(CONFIG_CLOCKSOURCE_EFI_X86) += efi_x86.o
 obj-$(CONFIG_CLOCKSOURCE_MVEBU)   += mvebu.o
 obj-$(CONFIG_CLOCKSOURCE_NOMADIK) += nomadik.o
 obj-$(CONFIG_CLOCKSOURCE_ORION)   += orion.o
diff --git a/drivers/clocksource/efi.c b/drivers/clocksource/efi.c
new file mode 100644
index 0000000000..89906c452e
--- /dev/null
+++ b/drivers/clocksource/efi.c
@@ -0,0 +1,110 @@
+/*
+ * Copyright (C) 2017 Jean-Christophe PLAGNIOL-VILLARD <plagnio@jcrosoft.com>
+ *
+ * Under GPL v2
+ */
+#include <common.h>
+#include <init.h>
+#include <driver.h>
+#include <clock.h>
+#include <efi.h>
+#include <efi/efi.h>
+#include <efi/efi-device.h>
+#include <linux/err.h>
+
+static uint64_t ticks = 1;
+static void *efi_cs_evt;
+
+static uint64_t efi_cs_read(void)
+{
+	return ticks;
+}
+
+static void efi_cs_inc(void *event, void *ctx)
+{
+	ticks++;
+}
+
+/* count ticks during a 1dms */
+static uint64_t ticks_freq(void)
+{
+	uint64_t ticks_start, ticks_end;
+
+	ticks_start = ticks;
+	BS->stall(1000);
+	ticks_end = ticks;
+
+	return (ticks_end - ticks_start) * 1000;
+}
+
+/* count ticks during a 20ms delay as on qemu x86_64 the max is 100Hz */
+static uint64_t ticks_freq_x86(void)
+{
+	uint64_t ticks_start, ticks_end;
+
+	ticks_start = ticks;
+	BS->stall(20 * 1000);
+	ticks_end = ticks;
+
+	return (ticks_end - ticks_start) * 50;
+}
+
+static int efi_cs_init(struct clocksource *cs)
+{
+	efi_status_t efiret;
+	uint64_t freq;
+
+	efiret = BS->create_event(EFI_EVT_TIMER | EFI_EVT_NOTIFY_SIGNAL,
+			EFI_TPL_CALLBACK, efi_cs_inc, NULL, &efi_cs_evt);
+
+	if (EFI_ERROR(efiret))
+		return -efi_errno(efiret);
+
+	efiret = BS->set_timer(efi_cs_evt, EFI_TIMER_PERIODIC, 10);
+	if (EFI_ERROR(efiret)) {
+		BS->close_event(efi_cs_evt);
+		return -efi_errno(efiret);
+	}
+
+	freq = 1000 * 1000;
+	if (ticks_freq() < 800 * 1000) {
+		uint64_t nb_100ns;
+
+		freq = ticks_freq_x86();
+		nb_100ns = 10 * 1000 * 1000 / freq;
+		pr_warn("EFI Event timer too slow freq = %llu Hz\n", freq);
+		efiret = BS->set_timer(efi_cs_evt, EFI_TIMER_PERIODIC, nb_100ns);
+		if (EFI_ERROR(efiret)) {
+			BS->close_event(efi_cs_evt);
+			return -efi_errno(efiret);
+		}
+	}
+
+	cs->mult = clocksource_hz2mult(freq, cs->shift);
+
+	return 0;
+}
+
+static struct clocksource efi_cs = {
+	.read   = efi_cs_read,
+	.mask   = CLOCKSOURCE_MASK(64),
+	.shift  = 0,
+	.init   = efi_cs_init,
+};
+
+static int efi_cs_probe(struct device_d *dev)
+{
+	return init_clock(&efi_cs);
+}
+
+static struct driver_d efi_cs_driver = {
+	.name = "efi-cs",
+	.probe = efi_cs_probe,
+};
+
+static int efi_cs_initcall(void)
+{
+	return platform_driver_register(&efi_cs_driver);
+}
+/* for efi the time must be init at core initcall level */
+core_initcall(efi_cs_initcall);
diff --git a/drivers/clocksource/efi_x86.c b/drivers/clocksource/efi_x86.c
new file mode 100644
index 0000000000..4d2657ea1d
--- /dev/null
+++ b/drivers/clocksource/efi_x86.c
@@ -0,0 +1,79 @@
+#include <common.h>
+#include <init.h>
+#include <driver.h>
+#include <efi.h>
+#include <efi/efi.h>
+#include <clock.h>
+
+#ifdef __x86_64__
+uint64_t ticks_read(void)
+{
+	uint64_t a, d;
+
+	__asm__ volatile ("rdtsc" : "=a" (a), "=d" (d));
+
+	return (d << 32) | a;
+}
+#else
+uint64_t ticks_read(void)
+{
+	uint64_t val;
+
+	__asm__ volatile ("rdtsc" : "=A" (val));
+
+	return val;
+}
+#endif
+
+static uint64_t freq;
+
+/* count TSC ticks during a millisecond delay */
+static uint64_t ticks_freq(void)
+{
+	uint64_t ticks_start, ticks_end;
+
+	ticks_start = ticks_read();
+	BS->stall(1000);
+	ticks_end = ticks_read();
+
+	return (ticks_end - ticks_start) * 1000;
+}
+
+static uint64_t efi_x86_cs_read(void)
+{
+	return 1000 * 1000 * ticks_read() / freq;
+}
+
+static int efi_x86_cs_init(struct clocksource *cs)
+{
+	cs->mult = clocksource_hz2mult(1000 * 1000, cs->shift);
+
+	freq = ticks_freq();
+
+	return 0;
+}
+
+static struct clocksource efi_x86_cs = {
+	.read   = efi_x86_cs_read,
+	.mask   = CLOCKSOURCE_MASK(64),
+	.shift  = 0,
+	.priority = 100,
+	.init   = efi_x86_cs_init,
+};
+
+static int efi_x86_cs_probe(struct device_d *dev)
+{
+	return init_clock(&efi_x86_cs);
+}
+
+static struct driver_d efi_x86_cs_driver = {
+	.name = "efi-cs-x86",
+	.probe = efi_x86_cs_probe,
+};
+
+static int efi_x86_cs_initcall(void)
+{
+	return platform_driver_register(&efi_x86_cs_driver);
+}
+/* for efi the time must be init at core initcall level */
+core_initcall(efi_x86_cs_initcall);
diff --git a/drivers/efi/Kconfig b/drivers/efi/Kconfig
new file mode 100644
index 0000000000..2cd9dd504f
--- /dev/null
+++ b/drivers/efi/Kconfig
@@ -0,0 +1,2 @@
+config EFI_BOOTUP
+	bool
diff --git a/drivers/efi/Makefile b/drivers/efi/Makefile
new file mode 100644
index 0000000000..de31212f25
--- /dev/null
+++ b/drivers/efi/Makefile
@@ -0,0 +1 @@
+obj-y += efi-device.o
diff --git a/drivers/efi/efi-device.c b/drivers/efi/efi-device.c
new file mode 100644
index 0000000000..6ed7f12b37
--- /dev/null
+++ b/drivers/efi/efi-device.c
@@ -0,0 +1,586 @@
+/*
+ * efi-device.c - barebox EFI payload support
+ *
+ * Copyright (c) 2014 Sascha Hauer <s.hauer@pengutronix.de>, Pengutronix
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <command.h>
+#include <common.h>
+#include <driver.h>
+#include <malloc.h>
+#include <memory.h>
+#include <string.h>
+#include <linux/sizes.h>
+#include <wchar.h>
+#include <init.h>
+#include <efi.h>
+#include <efi/efi.h>
+#include <efi/efi-device.h>
+#include <linux/err.h>
+
+int efi_locate_handle(enum efi_locate_search_type search_type,
+		efi_guid_t *protocol,
+		void *search_key,
+		unsigned long *no_handles,
+		efi_handle_t **buffer)
+{
+	efi_status_t efiret;
+	unsigned long buffer_size = 0;
+	efi_handle_t *buf;
+
+	efiret = BS->locate_handle(search_type, protocol, search_key, &buffer_size,
+			NULL);
+	if (EFI_ERROR(efiret) && efiret != EFI_BUFFER_TOO_SMALL)
+		return -efi_errno(efiret);
+
+	buf = malloc(buffer_size);
+	if (!buf)
+		return -ENOMEM;
+
+	efiret = BS->locate_handle(search_type, protocol, search_key, &buffer_size,
+			buf);
+	if (EFI_ERROR(efiret)) {
+		free(buf);
+		return -efi_errno(efiret);
+	}
+
+	*no_handles = buffer_size / sizeof(efi_handle_t);
+	*buffer = buf;
+
+	return 0;
+}
+
+static struct efi_device *efi_find_device(efi_handle_t *handle)
+{
+	struct device_d *dev;
+	struct efi_device *efidev;
+
+	bus_for_each_device(&efi_bus, dev) {
+		efidev = container_of(dev, struct efi_device, dev);
+
+		if (efidev->handle == handle)
+			return efidev;
+	}
+
+	return NULL;
+}
+
+static void efi_devinfo(struct device_d *dev)
+{
+	struct efi_device *efidev = to_efi_device(dev);
+	int i;
+
+	printf("Protocols:\n");
+
+	for (i = 0; i < efidev->num_guids; i++)
+		printf("  %d: %pUl: %s\n", i, &efidev->guids[i],
+					efi_guid_string(&efidev->guids[i]));
+}
+
+static efi_handle_t *efi_find_parent(efi_handle_t *handle)
+{
+	unsigned long handle_count = 0;
+        efi_handle_t *handles = NULL, *parent;
+	unsigned long num_guids;
+	efi_guid_t **guids;
+	int ret, i, j, k;
+	efi_status_t efiret;
+	struct efi_open_protocol_information_entry *entry_buffer;
+	unsigned long entry_count;
+
+	ret = efi_locate_handle(all_handles, NULL, NULL, &handle_count, &handles);
+	if (ret)
+		return NULL;
+
+	/*
+	 * Normally one would expect a function/pointer to retrieve the parent.
+	 * With EFI we have to:
+	 * - get all handles
+	 * - for each handle get the registered protocols
+	 * - for each protocol get the users
+	 * - the user which matches the input handle is the parent
+	 */
+	for (i = 0; i < handle_count; i++) {
+		efiret = BS->open_protocol(handles[i], &efi_device_path_protocol_guid,
+				NULL, NULL, NULL, EFI_OPEN_PROTOCOL_TEST_PROTOCOL);
+		if (EFI_ERROR(efiret))
+			continue;
+
+		BS->protocols_per_handle(handles[i], &guids, &num_guids);
+		for (j = 0; j < num_guids; j++) {
+			efiret = BS->open_protocol_information(handles[i], guids[j],
+				&entry_buffer, &entry_count);
+			for (k = 0; k < entry_count; k++) {
+				if (entry_buffer[k].controller_handle == NULL)
+					continue;
+				if (entry_buffer[k].controller_handle == handles[i])
+					continue;
+				if (entry_buffer[k].controller_handle == handle) {
+					parent = handles[i];
+					goto out;
+				}
+			}
+		}
+	}
+
+	parent = NULL;
+
+	free(handles);
+out:
+	return parent;
+}
+
+static struct efi_device *efi_add_device(efi_handle_t *handle, efi_guid_t **guids,
+		int num_guids)
+{
+	struct efi_device *efidev;
+	int i;
+	efi_guid_t *guidarr;
+	efi_status_t efiret;
+	void *devpath;
+
+	efidev = efi_find_device(handle);
+	if (efidev)
+		return ERR_PTR(-EEXIST);
+
+	efiret = BS->open_protocol(handle, &efi_device_path_protocol_guid,
+			NULL, NULL, NULL, EFI_OPEN_PROTOCOL_TEST_PROTOCOL);
+	if (EFI_ERROR(efiret))
+		return ERR_PTR(-EINVAL);
+
+	guidarr = malloc(sizeof(efi_guid_t) * num_guids);
+
+	for (i = 0; i < num_guids; i++)
+		memcpy(&guidarr[i], guids[i], sizeof(efi_guid_t));
+
+	efiret = BS->open_protocol(handle, &efi_device_path_protocol_guid,
+			&devpath, NULL, NULL, EFI_OPEN_PROTOCOL_GET_PROTOCOL);
+	if (EFI_ERROR(efiret)) {
+		free(guidarr);
+		return ERR_PTR(-EINVAL);
+	}
+
+	efidev = xzalloc(sizeof(*efidev));
+
+	efidev->guids = guidarr;
+	efidev->num_guids = num_guids;
+	efidev->handle = handle;
+	efidev->dev.bus = &efi_bus;
+	efidev->dev.id = DEVICE_ID_SINGLE;
+	efidev->dev.info = efi_devinfo;
+	efidev->devpath = devpath;
+
+	BS->handle_protocol(handle, &guidarr[0], &efidev->protocol);
+
+	sprintf(efidev->dev.name, "handle-%p", handle);
+
+	efidev->parent_handle = efi_find_parent(efidev->handle);
+
+	return efidev;
+}
+
+
+static int efi_register_device(struct efi_device *efidev)
+{
+	char *dev_path_str;
+	struct efi_device *parent;
+	int ret;
+
+	if (efi_find_device(efidev->handle))
+		return -EEXIST;
+
+	if (efidev->parent_handle) {
+		parent = efi_find_device(efidev->parent_handle);
+		if (!parent)
+			return -EINVAL;
+
+		efidev->dev.parent = &parent->dev;
+	}
+
+	ret = register_device(&efidev->dev);
+	if (ret)
+		return ret;
+
+	dev_path_str = device_path_to_str(efidev->devpath);
+	if (dev_path_str) {
+		dev_add_param_fixed(&efidev->dev, "devpath", dev_path_str);
+		free(dev_path_str);
+	}
+
+	debug("registered efi device %s\n", dev_name(&efidev->dev));
+
+	return 0;
+}
+
+/**
+ * efi_register_devices - iterate over all EFI handles and register
+ *                        the devices found
+ *
+ * in barebox we treat all EFI handles which support the device_path
+ * protocol as devices. This function iterates over all handles and
+ * registers the corresponding devices. efi_register_devices is safe
+ * to call multiple times. Already registered devices will be ignored.
+ *
+ */
+void efi_register_devices(void)
+{
+	unsigned long handle_count = 0;
+        efi_handle_t *handles = NULL;
+	unsigned long num_guids;
+	efi_guid_t **guids;
+	int ret, i;
+	struct efi_device **efidevs;
+	int registered;
+
+	ret = efi_locate_handle(all_handles, NULL, NULL, &handle_count, &handles);
+	if (ret)
+		return;
+
+	efidevs = xzalloc(handle_count * sizeof(struct efi_device *));
+
+	for (i = 0; i < handle_count; i++) {
+		BS->protocols_per_handle(handles[i], &guids, &num_guids);
+
+		efidevs[i] = efi_add_device(handles[i], guids, num_guids);
+	}
+
+	/*
+	 * We have a list of devices we want to register, but can only
+	 * register a device when all parents are registered already.
+	 * Do this by continiously iterating over the list until no
+	 * further devices are registered.
+	 */
+	do {
+		registered = 0;
+
+		for (i = 0; i < handle_count; i++) {
+			if (IS_ERR(efidevs[i]))
+				continue;
+
+			ret = efi_register_device(efidevs[i]);
+			if (!ret) {
+				efidevs[i] = ERR_PTR(-EEXIST);
+				registered = 1;
+			}
+		}
+	} while (registered);
+
+	free(efidevs);
+	free(handles);
+}
+
+int efi_connect_all(void)
+{
+	efi_status_t  efiret;
+	unsigned long handle_count;
+	efi_handle_t *handle_buffer;
+	int i;
+
+	efiret = BS->locate_handle_buffer(all_handles, NULL, NULL, &handle_count,
+			&handle_buffer);
+	if (EFI_ERROR(efiret))
+		return -efi_errno(efiret);
+
+	for (i = 0; i < handle_count; i++)
+		efiret = BS->connect_controller(handle_buffer[i], NULL, NULL, true);
+
+	if (handle_buffer)
+		BS->free_pool(handle_buffer);
+
+	return 0;
+}
+
+static int efi_bus_match(struct device_d *dev, struct driver_d *drv)
+{
+	struct efi_driver *efidrv = to_efi_driver(drv);
+	struct efi_device *efidev = to_efi_device(dev);
+	int i;
+
+	for (i = 0; i < efidev->num_guids; i++) {
+		if (!memcmp(&efidrv->guid, &efidev->guids[i], sizeof(efi_guid_t)))
+			return 0;
+	}
+
+	return 1;
+}
+
+static int efi_bus_probe(struct device_d *dev)
+{
+	struct efi_driver *efidrv = to_efi_driver(dev->driver);
+	struct efi_device *efidev = to_efi_device(dev);
+
+	return efidrv->probe(efidev);
+}
+
+static void efi_bus_remove(struct device_d *dev)
+{
+	struct efi_driver *efidrv = to_efi_driver(dev->driver);
+	struct efi_device *efidev = to_efi_device(dev);
+
+	if (efidrv->remove)
+		efidrv->remove(efidev);
+}
+
+struct bus_type efi_bus = {
+	.name = "efi",
+	.match = efi_bus_match,
+	.probe = efi_bus_probe,
+	.remove = efi_bus_remove,
+};
+
+static void efi_businfo(struct device_d *dev)
+{
+	int i;
+
+	printf("Tables:\n");
+	for (i = 0; i < efi_sys_table->nr_tables; i++) {
+		efi_config_table_t *t = &efi_sys_table->tables[i];
+
+		printf("  %d: %pUl: %s\n", i, &t->guid,
+					efi_guid_string(&t->guid));
+	}
+}
+
+static int efi_is_secure_boot(void)
+{
+	uint8_t *val;
+	int ret = 0;
+
+	val = efi_get_variable("SecureBoot", &efi_global_variable_guid, NULL);
+	if (!IS_ERR(val)) {
+		ret = *val;
+		free(val);
+	}
+
+	return ret != 1;
+}
+
+static int efi_is_setup_mode(void)
+{
+	uint8_t *val;
+	int ret = 0;
+
+	val = efi_get_variable("SetupMode", &efi_global_variable_guid, NULL);
+	if (!IS_ERR(val)) {
+		ret = *val;
+		free(val);
+	}
+
+	return ret != 1;
+}
+
+static int efi_init_devices(void)
+{
+	char *fw_vendor = NULL;
+	u16 sys_major = efi_sys_table->hdr.revision >> 16;
+	u16 sys_minor = efi_sys_table->hdr.revision & 0xffff;
+	int secure_boot = efi_is_secure_boot();
+	int setup_mode = efi_is_setup_mode();
+
+	fw_vendor = strdup_wchar_to_char((const wchar_t *)efi_sys_table->fw_vendor);
+
+	pr_info("EFI v%u.%.02u by %s v%u\n",
+		sys_major, sys_minor,
+		fw_vendor, efi_sys_table->fw_revision);
+
+	bus_register(&efi_bus);
+
+	dev_add_param_fixed(efi_bus.dev, "fw_vendor", fw_vendor);
+	free(fw_vendor);
+
+	dev_add_param_int_ro(efi_bus.dev, "major", sys_major, "%u");
+	dev_add_param_int_ro(efi_bus.dev, "minor", sys_minor, "%u");
+	dev_add_param_int_ro(efi_bus.dev, "fw_revision", efi_sys_table->fw_revision, "%u");
+	dev_add_param_int_ro(efi_bus.dev, "secure_boot", secure_boot, "%d");
+	dev_add_param_int_ro(efi_bus.dev, "secure_mode",
+			     secure_boot & setup_mode, "%u");
+
+	efi_bus.dev->info = efi_businfo;
+
+	efi_register_devices();
+
+	return 0;
+}
+core_initcall(efi_init_devices);
+
+static void efi_devpath(efi_handle_t handle)
+{
+	efi_status_t efiret;
+	void *devpath;
+	char *dev_path_str;
+
+	efiret = BS->open_protocol(handle, &efi_device_path_protocol_guid,
+				   &devpath, NULL, NULL, EFI_OPEN_PROTOCOL_GET_PROTOCOL);
+	if (EFI_ERROR(efiret))
+		return;
+
+	dev_path_str = device_path_to_str(devpath);
+	if (dev_path_str) {
+		printf("  Devpath: \n  %s\n", dev_path_str);
+		free(dev_path_str);
+	}
+}
+
+static void efi_dump(efi_handle_t *handles, unsigned long handle_count)
+{
+	int i, j;
+	unsigned long num_guids;
+	efi_guid_t **guids;
+
+	if (!handles || !handle_count)
+		return;
+
+	for (i = 0; i < handle_count; i++) {
+		printf("handle-%p\n", handles[i]);
+
+		BS->protocols_per_handle(handles[i], &guids, &num_guids);
+		printf("  Protocols:\n");
+		for (j = 0; j < num_guids; j++)
+			printf("  %d: %pUl: %s\n", j, guids[j],
+					efi_guid_string(guids[j]));
+		efi_devpath(handles[i]);
+	}
+	printf("\n");
+}
+
+static unsigned char to_digit(unsigned char c)
+{
+	if (c >= '0' && c <= '9')
+		c -= '0';
+	else if (c >= 'A' && c <= 'F')
+		c -= 'A' - 10;
+	else
+		c -= 'a' - 10;
+
+	return c;
+}
+
+#define read_xbit(src, dest, bit) 					\
+	do {								\
+		int __i;						\
+		for (__i = (bit - 4); __i >= 0; __i -= 4, src++)	\
+			dest |= to_digit(*src) << __i;			\
+	} while (0)
+
+static int do_efi_protocol_dump(int argc, char **argv)
+{
+	unsigned long handle_count = 0;
+	efi_handle_t *handles = NULL;
+	int ret;
+	efi_guid_t guid;
+	u32 a = 0;
+	u16 b = 0;
+	u16 c = 0;
+	u8 d0 = 0;
+	u8 d1 = 0;
+	u8 d2 = 0;
+	u8 d3 = 0;
+	u8 d4 = 0;
+	u8 d5 = 0;
+	u8 d6 = 0;
+	u8 d7 = 0;
+
+	/* Format 220e73b6-6bdb-4413-8405-b974b108619a */
+	if (argc == 1) {
+		char *s = argv[0];
+		int len = strlen(s);
+
+		if (len != 36)
+			return -EINVAL;
+		
+		read_xbit(s, a, 32);
+		if (*s != '-')
+			return -EINVAL;
+		s++;
+		read_xbit(s, b, 16);
+		if (*s != '-')
+			return -EINVAL;
+		s++;
+		read_xbit(s, c, 16);
+		if (*s != '-')
+			return -EINVAL;
+		s++;
+		read_xbit(s, d0, 8);
+		read_xbit(s, d1, 8);
+		if (*s != '-')
+			return -EINVAL;
+		s++;
+		read_xbit(s, d2, 8);
+		read_xbit(s, d3, 8);
+		read_xbit(s, d4, 8);
+		read_xbit(s, d5, 8);
+		read_xbit(s, d6, 8);
+		read_xbit(s, d7, 8);
+	} else if (argc == 11) {
+		/* Format :
+		 *	220e73b6 6bdb 4413 84 05 b9 74 b1 08 61 9a
+		 *   or
+		 *	0x220e73b6 0x6bdb 0x14413 0x84 0x05 0xb9 0x74 0xb1 0x08 0x61 0x9a
+		 */
+		a = simple_strtoul(argv[0], NULL, 16);
+		b = simple_strtoul(argv[1], NULL, 16);
+		c = simple_strtoul(argv[2], NULL, 16);
+		d0 = simple_strtoul(argv[3], NULL, 16);
+		d1 = simple_strtoul(argv[4], NULL, 16);
+		d2 = simple_strtoul(argv[5], NULL, 16);
+		d3 = simple_strtoul(argv[6], NULL, 16);
+		d4 = simple_strtoul(argv[7], NULL, 16);
+		d5 = simple_strtoul(argv[8], NULL, 16);
+		d6 = simple_strtoul(argv[9], NULL, 16);
+		d7 = simple_strtoul(argv[10], NULL, 16);
+	} else {
+		return -EINVAL;
+	}
+
+	guid = EFI_GUID(a, b, c, d0, d1, d2, d3, d4, d5, d6, d7);
+
+	printf("Searching for:\n");
+	printf("  %pUl: %s\n", &guid, efi_guid_string(&guid));
+
+	ret = efi_locate_handle(by_protocol, &guid, NULL, &handle_count, &handles);
+	if (!ret)
+		efi_dump(handles, handle_count);
+
+	return 0;
+}
+
+static int do_efi_handle_dump(int argc, char *argv[])
+{
+	unsigned long handle_count = 0;
+	efi_handle_t *handles = NULL;
+	int ret;
+
+	if (argc > 1)
+		return do_efi_protocol_dump(--argc, ++argv);
+
+	ret = efi_locate_handle(all_handles, NULL, NULL, &handle_count, &handles);
+	if (!ret)
+		efi_dump(handles, handle_count);
+
+	return 0;
+}
+
+BAREBOX_CMD_HELP_START(efi_handle_dump)
+BAREBOX_CMD_HELP_TEXT("Dump all the efi handle with protocol and devpath\n")
+BAREBOX_CMD_HELP_END
+
+BAREBOX_CMD_START(efi_handle_dump)
+	.cmd = do_efi_handle_dump,
+	BAREBOX_CMD_DESC("Usage: efi_handle_dump")
+	BAREBOX_CMD_OPTS("[a-b-c-d0d1-d3d4d5d6d7] or [a b c d0 d1 d2 d3 d4 d5 d6 d7]")
+	BAREBOX_CMD_GROUP(CMD_GRP_MISC)
+	BAREBOX_CMD_HELP(cmd_efi_handle_dump_help)
+BAREBOX_CMD_END
diff --git a/drivers/net/Kconfig b/drivers/net/Kconfig
index b723a127f2..c3980e78f5 100644
--- a/drivers/net/Kconfig
+++ b/drivers/net/Kconfig
@@ -202,7 +202,7 @@ config DRIVER_NET_TAP
 
 config DRIVER_NET_EFI_SNP
 	bool "EFI SNP ethernet driver"
-	depends on ARCH_EFI
+	depends on EFI_BOOTUP
 
 config DRIVER_NET_TSE
 	depends on NIOS2
diff --git a/drivers/net/efi-snp.c b/drivers/net/efi-snp.c
index 963d539db3..23edc9f04f 100644
--- a/drivers/net/efi-snp.c
+++ b/drivers/net/efi-snp.c
@@ -23,8 +23,8 @@
 #include <net.h>
 #include <init.h>
 #include <efi.h>
-#include <mach/efi.h>
-#include <mach/efi-device.h>
+#include <efi/efi.h>
+#include <efi/efi-device.h>
 
 struct efi_network_statistics {
 	uint64_t RxTotalFrames;
diff --git a/drivers/of/Kconfig b/drivers/of/Kconfig
index d0a62bda91..a1fac0e613 100644
--- a/drivers/of/Kconfig
+++ b/drivers/of/Kconfig
@@ -4,7 +4,7 @@ config OFTREE
 
 config OFTREE_MEM_GENERIC
 	depends on OFTREE
-	depends on PPC || ARM || ARCH_EFI || OPENRISC || SANDBOX
+	depends on PPC || ARM || EFI_BOOTUP || OPENRISC || SANDBOX
 	def_bool y
 
 config DTC
diff --git a/drivers/serial/Kconfig b/drivers/serial/Kconfig
index 4eab437ea5..cfddc2ee96 100644
--- a/drivers/serial/Kconfig
+++ b/drivers/serial/Kconfig
@@ -21,6 +21,10 @@ config DRIVER_SERIAL_AR933X
 	  If you have an Atheros AR933X SOC based board and want to use the
 	  built-in UART of the SoC, say Y to this option.
 
+config DRIVER_SERIAL_EFI
+	bool "EFI serial"
+	depends on EFI_BOOTUP
+
 config DRIVER_SERIAL_IMX
 	depends on ARCH_IMX
 	default y
@@ -46,7 +50,7 @@ config DRIVER_SERIAL_LINUX_CONSOLE
 	bool "linux console driver"
 
 config DRIVER_SERIAL_EFI_STDIO
-	depends on ARCH_EFI
+	depends on EFI_BOOTUP
 	bool "EFI stdio driver"
 
 config DRIVER_SERIAL_MPC5XXX
diff --git a/drivers/serial/Makefile b/drivers/serial/Makefile
index 7d1bae195f..3d9f735ed2 100644
--- a/drivers/serial/Makefile
+++ b/drivers/serial/Makefile
@@ -1,6 +1,7 @@
 obj-$(CONFIG_DRIVER_SERIAL_ARM_DCC)		+= arm_dcc.o
 obj-$(CONFIG_SERIAL_AMBA_PL011)			+= amba-pl011.o
 obj-$(CONFIG_DRIVER_SERIAL_AR933X)		+= serial_ar933x.o
+obj-$(CONFIG_DRIVER_SERIAL_EFI)			+= serial_efi.o
 obj-$(CONFIG_DRIVER_SERIAL_IMX)			+= serial_imx.o
 obj-$(CONFIG_DRIVER_SERIAL_STM378X)		+= stm-serial.o
 obj-$(CONFIG_DRIVER_SERIAL_ATMEL)		+= atmel.o
diff --git a/drivers/serial/efi-stdio.c b/drivers/serial/efi-stdio.c
index 5ab917386e..0703f727e7 100644
--- a/drivers/serial/efi-stdio.c
+++ b/drivers/serial/efi-stdio.c
@@ -25,7 +25,7 @@
 #include <efi.h>
 #include <readkey.h>
 #include <linux/ctype.h>
-#include <mach/efi.h>
+#include <efi/efi.h>
 
 #define EFI_SHIFT_STATE_VALID           0x80000000
 #define EFI_RIGHT_CONTROL_PRESSED       0x00000004
diff --git a/drivers/serial/serial_efi.c b/drivers/serial/serial_efi.c
new file mode 100644
index 0000000000..f0a2b22c2b
--- /dev/null
+++ b/drivers/serial/serial_efi.c
@@ -0,0 +1,221 @@
+/*
+ * Copyright (C) 2017 Jean-Christophe PLAGNIOL-VILLARD <plagnioj@jcrosoft.com>
+ *
+ * Under GPLv2 Only
+ */
+
+#include <common.h>
+#include <driver.h>
+#include <init.h>
+#include <malloc.h>
+#include <efi.h>
+#include <efi/efi.h>
+#include <efi/efi-device.h>
+
+/*
+ * define for Control bits, grouped by read only, write only, and read write
+ *
+ * Read Only
+ */
+#define EFI_SERIAL_CLEAR_TO_SEND        0x00000010
+#define EFI_SERIAL_DATA_SET_READY       0x00000020
+#define EFI_SERIAL_RING_INDICATE        0x00000040
+#define EFI_SERIAL_CARRIER_DETECT       0x00000080
+#define EFI_SERIAL_INPUT_BUFFER_EMPTY   0x00000100
+#define EFI_SERIAL_OUTPUT_BUFFER_EMPTY  0x00000200
+
+/*
+ * Write Only
+ */
+#define EFI_SERIAL_REQUEST_TO_SEND      0x00000002
+#define EFI_SERIAL_DATA_TERMINAL_READY  0x00000001
+
+/*
+ * Read Write
+ */
+#define EFI_SERIAL_HARDWARE_LOOPBACK_ENABLE     0x00001000
+#define EFI_SERIAL_SOFTWARE_LOOPBACK_ENABLE     0x00002000
+#define EFI_SERIAL_HARDWARE_FLOW_CONTROL_ENABLE 0x00004000
+
+typedef enum {
+	DefaultParity,
+	NoParity,
+	EvenParity,
+	OddParity,
+	MarkParity,
+	SpaceParity
+} efi_parity_type;
+
+typedef enum {
+	DefaultStopBits,
+	OneStopBit,
+	OneFiveStopBits,
+	TwoStopBits
+} efi_stop_bits_type;
+
+struct efi_serial_io_mode {
+	uint32_t controlmask;
+	uint32_t timeout;
+	uint64_t baudrate;
+	uint32_t receivefifodepth;
+	uint32_t databits;
+	uint32_t parity;
+	uint32_t stopbits;
+};
+
+struct efi_serial_io_protocol {
+	uint32_t revision;
+
+	efi_status_t (EFIAPI *reset) (struct efi_serial_io_protocol *This);
+	efi_status_t (EFIAPI *set_attributes) (struct efi_serial_io_protocol *This,
+			uint64_t baudrate, uint32_t receivefifodepth,
+			uint32_t timeout, efi_parity_type parity,
+			uint8_t databits, efi_stop_bits_type stopbits);
+	efi_status_t (EFIAPI *setcontrol) (struct efi_serial_io_protocol *This,
+			uint32_t control);
+	efi_status_t (EFIAPI *getcontrol) (struct efi_serial_io_protocol *This,
+			uint32_t *control);
+	efi_status_t (EFIAPI *write) (struct efi_serial_io_protocol *This,
+			unsigned long *buffersize, void *buffer);
+	efi_status_t (EFIAPI *read) (struct efi_serial_io_protocol *This,
+			unsigned long *buffersize, void *buffer);
+
+	struct efi_serial_io_mode *mode;
+};
+
+/*
+ * We wrap our port structure around the generic console_device.
+ */
+struct efi_serial_port {
+	struct efi_serial_io_protocol *serial;
+	struct console_device	uart;		/* uart */
+	struct efi_device *efidev;
+};
+
+static inline struct efi_serial_port *
+to_efi_serial_port(struct console_device *uart)
+{
+	return container_of(uart, struct efi_serial_port, uart);
+}
+
+static int efi_serial_setbaudrate(struct console_device *cdev, int baudrate)
+{
+	struct efi_serial_port *uart = to_efi_serial_port(cdev);
+	struct efi_serial_io_protocol *serial = uart->serial;
+	efi_status_t efiret;
+
+	efiret = serial->set_attributes(serial, baudrate, 0, 0, NoParity, 8,
+					OneStopBit);
+	if (EFI_ERROR(efiret))
+		return -efi_errno(efiret);
+
+	return 0;
+}
+
+static void efi_serial_putc(struct console_device *cdev, char c)
+{
+	struct efi_serial_port *uart = to_efi_serial_port(cdev);
+	struct efi_serial_io_protocol *serial = uart->serial;
+	uint32_t control;
+	efi_status_t efiret;
+	unsigned long buffersize = sizeof(char);
+
+	do {
+		efiret = serial->getcontrol(serial, &control);
+		if (EFI_ERROR(efiret))
+			return;
+
+	} while(!(control & EFI_SERIAL_CLEAR_TO_SEND));
+
+	serial->write(serial, &buffersize, &c);
+}
+
+static int efi_serial_puts(struct console_device *cdev, const char *s)
+{
+	struct efi_serial_port *uart = to_efi_serial_port(cdev);
+	struct efi_serial_io_protocol *serial = uart->serial;
+	uint32_t control;
+	efi_status_t efiret;
+	unsigned long buffersize = strlen(s) * sizeof(char);
+
+	do {
+		efiret = serial->getcontrol(serial, &control);
+		if (EFI_ERROR(efiret))
+			return 0;
+
+	} while(!(control & EFI_SERIAL_CLEAR_TO_SEND));
+
+	serial->write(serial, &buffersize, (void*)s);
+
+	return strlen(s);
+}
+
+static int efi_serial_getc(struct console_device *cdev)
+{
+	struct efi_serial_port *uart = to_efi_serial_port(cdev);
+	struct efi_serial_io_protocol *serial = uart->serial;
+	uint32_t control;
+	efi_status_t efiret;
+	unsigned long buffersize = sizeof(char);
+	char c;
+
+	do {
+		efiret = serial->getcontrol(serial, &control);
+		if (EFI_ERROR(efiret))
+			return (int)-1;
+
+	} while(!(control & EFI_SERIAL_DATA_SET_READY));
+
+	serial->read(serial, &buffersize, &c);
+
+	return (int)c;
+}
+
+static int efi_serial_tstc(struct console_device *cdev)
+{
+	struct efi_serial_port *uart = to_efi_serial_port(cdev);
+	struct efi_serial_io_protocol *serial = uart->serial;
+	uint32_t control;
+	efi_status_t efiret;
+
+	efiret = serial->getcontrol(serial, &control);
+	if (EFI_ERROR(efiret))
+		return 0;
+
+	return !(control & EFI_SERIAL_INPUT_BUFFER_EMPTY);
+}
+
+static int efi_serial_probe(struct efi_device *efidev)
+{
+	struct efi_serial_port *uart;
+	struct console_device *cdev;
+
+	uart = xzalloc(sizeof(struct efi_serial_port));
+
+	cdev = &uart->uart;
+	cdev->dev = &efidev->dev;
+	cdev->tstc = efi_serial_tstc;
+	cdev->putc = efi_serial_putc;
+	cdev->puts = efi_serial_puts;
+	cdev->getc = efi_serial_getc;
+	cdev->setbrg = efi_serial_setbaudrate;
+
+	uart->serial = efidev->protocol;
+
+	uart->serial->reset(uart->serial);
+
+	/* Enable UART */
+
+	console_register(cdev);
+
+	return 0;
+}
+
+static struct efi_driver efi_serial_driver = {
+        .driver = {
+		.name  = "efi-serial",
+	},
+        .probe = efi_serial_probe,
+	.guid = EFI_SERIAL_IO_PROTOCOL_GUID,
+};
+device_efi_driver(efi_serial_driver);
diff --git a/drivers/video/Kconfig b/drivers/video/Kconfig
index 8f31f5af74..8d50db6f61 100644
--- a/drivers/video/Kconfig
+++ b/drivers/video/Kconfig
@@ -24,6 +24,10 @@ config DRIVER_VIDEO_ATMEL_HLCD
 	bool "Atmel HLCDC framebuffer driver"
 	depends on ARCH_AT91
 
+config DRIVER_VIDEO_EFI_GOP
+	bool "EFI Graphics Output Protocol (GOP)"
+	depends on EFI_BOOTUP
+
 config DRIVER_VIDEO_IMX
 	bool "i.MX framebuffer driver"
 	depends on ARCH_IMX1 || ARCH_IMX21 || ARCH_IMX25 || ARCH_IMX27
diff --git a/drivers/video/Makefile b/drivers/video/Makefile
index 1bf2e1f3ca..97712182e2 100644
--- a/drivers/video/Makefile
+++ b/drivers/video/Makefile
@@ -21,3 +21,5 @@ obj-$(CONFIG_DRIVER_VIDEO_OMAP) += omap.o
 obj-$(CONFIG_DRIVER_VIDEO_BCM283X) += bcm2835.o
 obj-$(CONFIG_DRIVER_VIDEO_SIMPLEFB) += simplefb.o
 obj-$(CONFIG_DRIVER_VIDEO_IMX_IPUV3) += imx-ipu-v3/
+
+obj-$(CONFIG_DRIVER_VIDEO_EFI_GOP) += efi_gop.o
diff --git a/drivers/video/efi_gop.c b/drivers/video/efi_gop.c
new file mode 100644
index 0000000000..7c083e4fb3
--- /dev/null
+++ b/drivers/video/efi_gop.c
@@ -0,0 +1,267 @@
+/*
+ * Copyright 2011 Intel Corporation; author Matt Fleming
+ * Copyright (c) 2017 Jean-Christophe PLAGNIOL-VILLARD <plagnioj@jcrosoft.com>
+ *
+ * GPL v2
+ */
+
+#include <common.h>
+#include <driver.h>
+#include <init.h>
+#include <malloc.h>
+#include <fb.h>
+#include <errno.h>
+#include <gui/graphic_utils.h>
+#include <efi.h>
+#include <efi/efi.h>
+#include <efi/efi-device.h>
+
+#define PIXEL_RGB_RESERVED_8BIT_PER_COLOR		0
+#define PIXEL_BGR_RESERVED_8BIT_PER_COLOR		1
+#define PIXEL_BIT_MASK					2
+#define PIXEL_BLT_ONLY					3
+#define PIXEL_FORMAT_MAX				4
+
+struct efi_pixel_bitmask {
+	u32 red_mask;
+	u32 green_mask;
+	u32 blue_mask;
+	u32 reserved_mask;
+};
+
+struct efi_graphics_output_mode_info {
+	u32 version;
+	u32 horizontal_resolution;
+	u32 vertical_resolution;
+	int pixel_format;
+	struct efi_pixel_bitmask pixel_information;
+	u32 pixels_per_scan_line;
+};
+
+struct efi_graphics_output_protocol_mode {
+	uint32_t max_mode;
+	uint32_t mode;
+	struct efi_graphics_output_mode_info *info;
+	unsigned long size_of_info;
+	void *frame_buffer_base;
+	unsigned long frame_buffer_size;
+};
+
+struct efi_graphics_output_protocol {
+	efi_status_t (EFIAPI *query_mode) (struct efi_graphics_output_protocol *This,
+			uint32_t mode_number, unsigned long *size_of_info,
+			struct efi_graphics_output_mode_info **info);
+	efi_status_t (EFIAPI *set_mode) (struct efi_graphics_output_protocol *This,
+			uint32_t mode_number);
+	efi_status_t (EFIAPI *blt)(struct efi_graphics_output_protocol *This,
+			void *buffer,
+			unsigned long operation,
+			unsigned long sourcex, unsigned long sourcey,
+			unsigned long destinationx, unsigned long destinationy,
+			unsigned long width, unsigned long height, unsigned
+			long delta);
+	struct efi_graphics_output_protocol_mode *mode;
+};
+
+struct efi_gop_priv {
+	struct device_d *dev;
+	struct fb_info fb;
+
+	uint32_t mode;
+	struct efi_graphics_output_protocol *gop;
+};
+
+static void find_bits(unsigned long mask, u32 *pos, u32 *size)
+{
+	u8 first, len;
+
+	first = 0;
+	len = 0;
+
+	if (mask) {
+		while (!(mask & 0x1)) {
+			mask = mask >> 1;
+			first++;
+		}
+
+		while (mask & 0x1) {
+			mask = mask >> 1;
+			len++;
+		}
+	}
+
+	*pos = first;
+	*size = len;
+}
+
+static void setup_pixel_info(struct fb_info *fb, u32 pixels_per_scan_line,
+		 struct efi_pixel_bitmask pixel_info, int pixel_format)
+{
+	if (pixel_format == PIXEL_RGB_RESERVED_8BIT_PER_COLOR) {
+		fb->bits_per_pixel = 32;
+		fb->line_length = pixels_per_scan_line * 4;
+		fb->red.length = 8;
+		fb->red.offset = 0;
+		fb->green.length = 8;
+		fb->green.offset = 8;
+		fb->blue.length = 8;
+		fb->blue.offset = 16;
+		fb->transp.length = 8;
+		fb->transp.offset = 24;
+	} else if (pixel_format == PIXEL_BGR_RESERVED_8BIT_PER_COLOR) {
+		fb->bits_per_pixel = 32;
+		fb->line_length = pixels_per_scan_line * 4;
+		fb->red.length = 8;
+		fb->red.offset = 16;
+		fb->green.length = 8;
+		fb->green.offset = 8;
+		fb->blue.length = 8;
+		fb->blue.offset = 0;
+		fb->transp.length = 8;
+		fb->transp.offset = 24;
+	} else if (pixel_format == PIXEL_BIT_MASK) {
+		find_bits(pixel_info.red_mask, &fb->red.offset, &fb->red.length);
+		find_bits(pixel_info.green_mask, &fb->green.offset,
+			  &fb->green.length);
+		find_bits(pixel_info.blue_mask, &fb->blue.offset, &fb->blue.length);
+		find_bits(pixel_info.reserved_mask, &fb->transp.offset,
+			  &fb->transp.length);
+		fb->bits_per_pixel = fb->red.length + fb->green.length +
+			fb->blue.length + fb->transp.length;
+		fb->line_length = (pixels_per_scan_line * fb->bits_per_pixel) / 8;
+	} else {
+		fb->bits_per_pixel = 4;
+		fb->line_length = fb->xres / 2;
+		fb->red.length = 0;
+		fb->red.offset = 0;
+		fb->green.length = 0;
+		fb->green.offset = 0;
+		fb->blue.length = 0;
+		fb->blue.offset = 0;
+		fb->transp.length = 0;
+		fb->transp.offset = 0;
+	}
+}
+
+static int efi_gop_query(struct efi_gop_priv *priv)
+{
+	struct efi_graphics_output_protocol_mode *mode;
+	struct efi_graphics_output_mode_info *info;
+	efi_status_t efiret;
+	unsigned long size = 0;
+	int i;
+	struct fb_videomode *vmode;
+
+	mode = priv->gop->mode;
+	vmode = xzalloc(sizeof(*vmode) * mode->max_mode);
+
+	priv->fb.modes.num_modes = mode->max_mode;
+	priv->fb.modes.modes = vmode;
+
+	for (i = 0; i < mode->max_mode; i++, vmode++) {
+		efiret = priv->gop->query_mode(priv->gop, i, &size, &info);
+		if (EFI_ERROR(efiret))
+			continue;
+
+		vmode->name = basprintf("%d", i);
+		vmode->xres = info->horizontal_resolution;
+		vmode->yres = info->vertical_resolution;
+	}
+
+	priv->fb.screen_base = mode->frame_buffer_base;
+	priv->mode = mode->mode;
+	priv->fb.xres = priv->fb.mode->xres;
+	priv->fb.yres = priv->fb.mode->yres;
+
+	return 0;
+}
+
+static int efi_gop_fb_activate_var(struct fb_info *fb_info)
+{
+	struct efi_gop_priv *priv = fb_info->priv;
+	struct efi_graphics_output_mode_info *info;
+	int num;
+	unsigned long size = 0;
+	efi_status_t efiret;
+
+	num = simple_strtoul(fb_info->mode->name, NULL, 0);
+
+	if (priv->mode != num) {
+		efiret = priv->gop->set_mode(priv->gop, num);
+		if (EFI_ERROR(efiret))
+			return -efi_errno(efiret);
+		priv->mode = num;
+	}
+
+	efiret = priv->gop->query_mode(priv->gop, num, &size, &info);
+	if (EFI_ERROR(efiret))
+		return -efi_errno(efiret);
+
+	setup_pixel_info(&priv->fb, info->pixels_per_scan_line,
+			 info->pixel_information, info->pixel_format);
+
+	return 0;
+}
+
+static struct fb_ops efi_gop_ops = {
+	.fb_activate_var = efi_gop_fb_activate_var,
+};
+
+static int efi_gop_probe(struct efi_device *efidev)
+{
+	struct efi_gop_priv *priv;
+	int ret = 0;
+	efi_status_t efiret;
+	efi_guid_t got_guid = EFI_GRAPHICS_OUTPUT_PROTOCOL_GUID;
+	void *protocol;
+
+	efiret = BS->handle_protocol(efidev->handle, &got_guid, &protocol);
+	if (EFI_ERROR(efiret))
+		return  -efi_errno(efiret);
+
+	priv = xzalloc(sizeof(struct efi_gop_priv));
+	priv->gop = protocol;
+	priv->dev = &efidev->dev;
+
+	if (!priv->gop) {
+		ret = -EINVAL;
+		goto err;
+	}
+
+	ret = efi_gop_query(priv);
+	if (ret)
+		goto err;
+
+	priv->fb.priv = priv;
+	priv->fb.dev.parent = priv->dev;
+	priv->fb.fbops = &efi_gop_ops;
+	priv->fb.p_enable = 1;
+	priv->fb.current_mode = priv->mode;
+
+	ret = register_framebuffer(&priv->fb);
+	if (!ret) {
+		priv->dev->priv = &priv->fb;
+		return 0;
+	}
+
+	if (priv->fb.modes.modes) {
+		int i;
+
+		for (i = 0; i < priv->fb.modes.num_modes; i++)
+			free((void*)priv->fb.modes.modes[i].name);
+
+		free((void*)priv->fb.modes.modes);
+	}
+err:
+	free(priv);
+        return ret;
+}
+
+static struct efi_driver efi_gop_driver = {
+        .driver = {
+		.name  = "efi-gop",
+	},
+	.probe = efi_gop_probe,
+	.guid = EFI_GRAPHICS_OUTPUT_PROTOCOL_GUID,
+};
+device_efi_driver(efi_gop_driver);