-#ifdef DEBUG_EFI
- printf("%s:%d Jumping to 0x%lx\n", __func__, __LINE__, (long)entry);
+ debug("%s:%d Jumping to 0x%lx\n", __func__, __LINE__, (long)entry);
+
+ if (setjmp(&loaded_image_info.exit_jmp)) {
+ efi_status_t status = loaded_image_info.exit_status;
+ return status == EFI_SUCCESS ? 0 : -EINVAL;
+ }
+
+#ifdef CONFIG_ARM64
+ /* On AArch64 we need to make sure we call our payload in < EL3 */
+ if (current_el() == 3) {
+ smp_kick_all_cpus();
+ dcache_disable(); /* flush cache before switch to EL2 */
+
+ /* Move into EL2 and keep running there */
+ armv8_switch_to_el2((ulong)entry, (ulong)&loaded_image_info,
+ (ulong)&systab, 0, (ulong)efi_run_in_el2,
+ ES_TO_AARCH64);
+
+ /* Should never reach here, efi exits with longjmp */
+ while (1) { }
+ }