2 files changed, 41 insertions, 21 deletions

diff --git a/include/efi_loader.h b/include/efi_loader.h
index eb16c14b699..4262d0ac6b9 100644
--- a/include/efi_loader.h
+++ b/include/efi_loader.h
@@ -15,11 +15,14 @@
 
 #include <linux/list.h>
 
+int __efi_entry_check(void);
+int __efi_exit_check(void);
+
 /*
  * Enter the u-boot world from UEFI:
  */
 #define EFI_ENTRY(format, ...) do { \
-	efi_restore_gd(); \
+	assert(__efi_entry_check()); \
 	debug("EFI: Entry %s(" format ")\n", __func__, ##__VA_ARGS__); \
 	} while(0)
 
@@ -29,7 +32,8 @@
 #define EFI_EXIT(ret) ({ \
 	efi_status_t _r = ret; \
 	debug("EFI: Exit: %s: %u\n", __func__, (u32)(_r & ~EFI_ERROR_MASK)); \
-	efi_exit_func(_r); \
+	assert(__efi_exit_check()); \
+	_r; \
 	})
 
 /*
@@ -37,9 +41,9 @@
  */
 #define EFI_CALL(exp) do { \
 	debug("EFI: Call: %s\n", #exp); \
-	efi_exit_func(EFI_SUCCESS); \
+	assert(__efi_exit_check()); \
 	exp; \
-	efi_restore_gd(); \
+	assert(__efi_entry_check()); \
 	debug("EFI: Return From: %s\n", #exp); \
 	} while(0)
 
@@ -139,10 +143,9 @@ void efi_timer_check(void);
 void *efi_load_pe(void *efi, struct efi_loaded_image *loaded_image_info);
 /* Called once to store the pristine gd pointer */
 void efi_save_gd(void);
-/* Called from EFI_ENTRY on callback entry to put gd into the gd register */
+/* Special case handler for error/abort that just tries to dtrt to get
+ * back to u-boot world */
 void efi_restore_gd(void);
-/* Called from EFI_EXIT on callback exit to restore the gd register */
-efi_status_t efi_exit_func(efi_status_t ret);
 /* Call this to relocate the runtime section to an address space */
 void efi_runtime_relocate(ulong offset, struct efi_mem_desc *map);
 /* Call this to set the current device name */
diff --git a/lib/efi_loader/efi_boottime.c b/lib/efi_loader/efi_boottime.c
index 849d2298213..aa8d0d12d01 100644
--- a/lib/efi_loader/efi_boottime.c
+++ b/lib/efi_loader/efi_boottime.c
@@ -49,6 +49,30 @@ static struct efi_configuration_table __efi_runtime_data efi_conf_table[2];
 static volatile void *efi_gd, *app_gd;
 #endif
 
+static int entry_count;
+
+/* Called on every callback entry */
+int __efi_entry_check(void)
+{
+	int ret = entry_count++ == 0;
+#ifdef CONFIG_ARM
+	assert(efi_gd);
+	app_gd = gd;
+	gd = efi_gd;
+#endif
+	return ret;
+}
+
+/* Called on every callback exit */
+int __efi_exit_check(void)
+{
+	int ret = --entry_count == 0;
+#ifdef CONFIG_ARM
+	gd = app_gd;
+#endif
+	return ret;
+}
+
 /* Called from do_bootefi_exec() */
 void efi_save_gd(void)
 {
@@ -57,30 +81,21 @@ void efi_save_gd(void)
 #endif
 }
 
-/* Called on every callback entry */
+/*
+ * Special case handler for error/abort that just forces things back
+ * to u-boot world so we can dump out an abort msg, without any care
+ * about returning back to UEFI world.
+ */
 void efi_restore_gd(void)
 {
 #ifdef CONFIG_ARM
 	/* Only restore if we're already in EFI context */
 	if (!efi_gd)
 		return;
-
-	if (gd != efi_gd)
-		app_gd = gd;
 	gd = efi_gd;
 #endif
 }
 
-/* Called on every callback exit */
-efi_status_t efi_exit_func(efi_status_t ret)
-{
-#ifdef CONFIG_ARM
-	gd = app_gd;
-#endif
-
-	return ret;
-}
-
 /* Low 32 bit */
 #define EFI_LOW32(a) (a & 0xFFFFFFFFULL)
 /* High 32 bit */
@@ -733,7 +748,9 @@ static efi_status_t EFIAPI efi_start_image(efi_handle_t image_handle,
 		return EFI_EXIT(info->exit_status);
 	}
 
+	__efi_exit_check();
 	entry(image_handle, &systab);
+	__efi_entry_check();
 
 	/* Should usually never get here */
 	return EFI_EXIT(EFI_SUCCESS);