fallback: fix crash when boot entry index >= size of BootOrder list

Seen on Dell PowerEdge R740 when booting with BOOTX64 constantly.

This patch keeps the behaviour previous to commit #1b30c2b by returning
the index of the "Linux" entry.
Then a check is made to find the entry in the current BootOrder:
- if it isn't there, prepend the entry and copy the rest (this enlarges
  the BootOrder array by 1)
- if it's there, prepend the entry and copy all remaining entries

------------------------------------------------------------------------------

Example of outputs on a Dell PowerEdge R740:
- 0000 is BOOTX64 entry
- other entries are Dell's default ones (internal, no "Linux" there)

1. Entry not already existing in BootOrder

----
set_boot_order:486: Original nbootorder: 3
Original BootOrder: 0000 0003 0004
 :
add_to_boot_list:578: device path: "HD(1,GPT,99D47E76-590F-48FD-8FD6-0A0CE790D635)/\EFI\redhat\shimx64.efi"
find_boot_option:454: Found boot entry "Boot0005" with label "Red Hat Enterprise Linux" for file "\EFI\redhat\shimx64.efi"
add_to_boot_list:623: New nbootorder: 4
BootOrder: 0005 0000 0003 0004
find_boot_options:937: Found directory named "Dell"
update_boot_order:509: nbootorder: 4
BootOrder: 0005 0000 0003 0004
----

2. Entry not existing at all

----
set_boot_order:486: Original nbootorder: 3
Original BootOrder: 0000 0001 0002
 :
add_to_boot_list:578: device path: "HD(1,GPT,99D47E76-590F-48FD-8FD6-0A0CE790D635)/\EFI\redhat\shimx64.efi"
add_boot_option:245: Creating boot entry "Boot0005" with label "Red Hat Enterprise Linux" for file "\EFI\redhat\shimx64.efi"
add_boot_option:282: nbootorder: 4
BootOrder: 0005 0000 0001 0002
find_boot_options:937: Found directory named "Dell"
update_boot_order:509: nbootorder: 4
BootOrder: 0005 0000 0001 0002
----

3. Entry already existing in BootOrder

----
set_boot_order:486: Original nbootorder: 4
Original BootOrder: 0000 0005 0001 0002
 :
add_to_boot_list:578: device path: "HD(1,GPT,99D47E76-590F-48FD-8FD6-0A0CE790D635)/\EFI\redhat\shimx64.efi"
find_boot_option:454: Found boot entry "Boot0005" with label "Red Hat Enterprise Linux" for file "\EFI\redhat\shimx64.efi"
add_to_boot_list:623: New nbootorder: 4
BootOrder: 0005 0000 0001 0002
find_boot_options:937: Found directory named "Dell"
update_boot_order:509: nbootorder: 4
BootOrder: 0005 0000 0001 0002
----

1 files changed, 44 insertions, 12 deletions

diff --git a/fallback.c b/fallback.c
index 08748d57..d2c73acd 100644
--- a/fallback.c
+++ b/fallback.c
@@ -569,7 +569,7 @@ add_to_boot_list(CHAR16 *dirname, CHAR16 *filename, CHAR16 *label, CHAR16 *argum
 	full_device_path = FileDevicePath(this_image->DeviceHandle, fullpath);
 	if (!full_device_path) {
 		efi_status = EFI_OUT_OF_RESOURCES;
-		goto err;
+		goto done;
 	}
 	dps = DevicePathToStr(full_device_path);
 	VerbosePrint(L"file DP: %s\n", dps);
@@ -583,7 +583,7 @@ add_to_boot_list(CHAR16 *dirname, CHAR16 *filename, CHAR16 *label, CHAR16 *argum
 			dp = full_device_path;
 		} else {
 			efi_status = EFI_OUT_OF_RESOURCES;
-			goto err;
+			goto done;
 		}
 	}
 
@@ -612,21 +612,53 @@ add_to_boot_list(CHAR16 *dirname, CHAR16 *filename, CHAR16 *label, CHAR16 *argum
 	if (EFI_ERROR(efi_status)) {
 		add_boot_option(dp, full_device_path, fullpath, label,
 				arguments);
-	} else if (option != 0) {
-		CHAR16 *newbootorder;
-		newbootorder = AllocateZeroPool(sizeof (CHAR16) * nbootorder);
-		if (!newbootorder)
-			return EFI_OUT_OF_RESOURCES;
+		goto done;
+	}
 
-		newbootorder[0] = bootorder[option];
-		CopyMem(newbootorder + 1, bootorder, sizeof (CHAR16) * option);
-		CopyMem(newbootorder + option + 1, bootorder + option + 1,
-			sizeof (CHAR16) * (nbootorder - option - 1));
+	UINT16 bootnum;
+	CHAR16 *newbootorder;
+	/* Search for the option in the current bootorder */
+	for (bootnum = 0; bootnum < nbootorder; bootnum++)
+		if (bootorder[bootnum] == option)
+			break;
+	if (bootnum == nbootorder) {
+		/* Option not found, prepend option and copy the rest */
+		newbootorder = AllocateZeroPool(sizeof(CHAR16)
+						* (nbootorder + 1));
+		if (!newbootorder) {
+			efi_status = EFI_OUT_OF_RESOURCES;
+			goto done;
+		}
+		newbootorder[0] = option;
+		CopyMem(newbootorder + 1, bootorder,
+			sizeof(CHAR16) * nbootorder);
+		FreePool(bootorder);
+		bootorder = newbootorder;
+		nbootorder += 1;
+	} else {
+		/* Option found, put first and slice the rest */
+		newbootorder = AllocateZeroPool(
+			sizeof(CHAR16) * nbootorder);
+		if (!newbootorder) {
+			efi_status = EFI_OUT_OF_RESOURCES;
+			goto done;
+		}
+		newbootorder[0] = option;
+		CopyMem(newbootorder + 1, bootorder,
+			sizeof(CHAR16) * bootnum);
+		CopyMem(newbootorder + 1 + bootnum,
+			bootorder + bootnum + 1,
+			sizeof(CHAR16) * (nbootorder - bootnum - 1));
 		FreePool(bootorder);
 		bootorder = newbootorder;
 	}
+	VerbosePrint(L"New nbootorder: %d\nBootOrder: ",
+		      nbootorder);
+	for (int i = 0 ; i < nbootorder ; i++)
+		VerbosePrintUnprefixed(L"%04x ", bootorder[i]);
+	VerbosePrintUnprefixed(L"\n");
 
-err:
+done:
 	if (full_device_path)
 		FreePool(full_device_path);
 	if (dp && dp != full_device_path)