--- zzzz-none-000/linux-4.9.279/drivers/firmware/efi/libstub/efi-stub-helper.c	2021-08-08 06:38:54.000000000 +0000
+++ puma7-atom-6591-750/linux-4.9.279/drivers/firmware/efi/libstub/efi-stub-helper.c	2023-02-08 11:43:42.000000000 +0000
@@ -825,3 +825,250 @@
 fail:
 	return status;
 }
+
+efi_status_t efi_get_variable_early(efi_system_table_t *sys_table_arg,
+				    efi_guid_t *guid, const char *varname,
+				    size_t *size, u8 *data)
+{
+	efi_get_variable_t *f_getvar = sys_table_arg->runtime->get_variable;
+	struct efi_variable efivar;
+	efi_status_t status;
+	int len, i;
+
+	efivar.DataSize = sizeof(efivar.Data);
+
+	len = strlen(varname);
+	if (len >= ARRAY_SIZE(efivar.VariableName))
+		return EFI_INVALID_PARAMETER;
+
+	for (i = 0; i < len; i++)
+		efivar.VariableName[i] = varname[i];
+
+	efivar.VariableName[i] = '\0';
+
+	status =  f_getvar(efivar.VariableName, guid,
+			    NULL, &efivar.DataSize, efivar.Data);
+	if (status != EFI_SUCCESS)
+		return status;
+
+	if (efivar.DataSize > *size)
+		status = EFI_BUFFER_TOO_SMALL;
+	else
+		memcpy(data, efivar.Data, efivar.DataSize);
+
+	*size = efivar.DataSize;
+
+	return status;
+}
+
+#define efi_file_protocol_resolve_func(fh, func) (efi_is_64bit() ? \
+	(unsigned long)((efi_file_handle_64_t *)(fh))->func : \
+	(unsigned long)((efi_file_handle_32_t *)(fh))->func)
+
+bool efi_file_exists(void *_fh, efi_char16_t *filename_16)
+{
+	unsigned long efi_open = efi_file_protocol_resolve_func(_fh, open);
+	efi_status_t status;
+	void *h;
+
+	status = __efi_call_early(efi_open, _fh, &h, filename_16,
+				  EFI_FILE_MODE_READ, (u64)0);
+	if (status == EFI_SUCCESS) {
+		efi_file_close(h);
+		return true;
+	}
+
+	return false;
+}
+
+static unsigned int atou(const char *s)
+{
+	unsigned int i = 0;
+
+	while (*s >= '0' && *s <= '9')
+		i = i * 10 + (*s++ - '0');
+
+	return i;
+}
+
+static size_t efi_utoa(efi_char16_t *s, unsigned int val)
+{
+	efi_char16_t digits[16];
+	efi_char16_t *p = digits;
+	size_t len = 0;
+
+	do {
+		*p++ = '0' + (val % 10);
+	} while (val /= 10);
+
+	len = p - digits;
+
+	p--;
+	while (p >= digits)
+		*s++ = *p--;
+
+	*s = '\0';
+
+	return len;
+}
+
+static size_t efi_strcpy(efi_char16_t *dst, const efi_char16_t *src)
+{
+	efi_char16_t *dst0 = dst;
+
+	while (*src)
+		*dst++ = *src++;
+
+	*dst = '\0';
+
+	return dst - dst0;
+}
+
+static size_t efi_strlen(const efi_char16_t *str)
+{
+	const efi_char16_t *str0;
+
+	for (str0 = str; *str; str++)
+		;
+
+	return str - str0;
+}
+
+#define EFI_AVM_DTB_FILENAME_BASE L"efi\\boot\\Fritz_Box_HW"
+
+static efi_status_t avm_build_dtb_name_for(int hwrev, int hwsubrev,
+					   efi_char16_t *path, size_t size)
+{
+	efi_char16_t tail[16];
+	size_t len = 0;
+
+	len += efi_utoa(&tail[len], hwrev);
+	tail[len++] = '-';
+	len += efi_utoa(&tail[len], hwsubrev);
+
+	len += efi_strcpy(&tail[len], L".dtb");
+
+	if (efi_strlen(EFI_AVM_DTB_FILENAME_BASE) + len >= size)
+		return EFI_BUFFER_TOO_SMALL;
+
+	len = efi_strcpy(path, EFI_AVM_DTB_FILENAME_BASE);
+	efi_strcpy(&path[len], tail);
+
+	return EFI_SUCCESS;
+}
+
+/*
+ * Check the efi/boot folder for a suitable DTB and load it.
+ *
+ * If no DTB filename matches the HW-Subrevision, it will look up DTBs for
+ * older subrevisions. If none is found, the function fails.
+ */
+efi_status_t avm_get_dtb_setup_data(efi_system_table_t *sys_table_arg,
+			      efi_loaded_image_t *image,
+			      struct setup_data **dtb_setup_data)
+{
+	struct setup_data *setup_data;
+	efi_char16_t filename_16[64];
+	struct file_info dtb_file;
+	efi_file_handle_t *fh;
+	efi_status_t status;
+	unsigned long size;
+	size_t efi_varsiz;
+	u8 efi_var[16];
+	int hwsubrev;
+	ssize_t len;
+	__u8 *data;
+	int hwrev;
+
+	if (!dtb_setup_data)
+		return EFI_INVALID_PARAMETER;
+
+	efi_varsiz = sizeof(efi_var) - 1;
+	status = efi_get_variable_early(sys_table_arg, &EFI_AVM_VARIABLE_GUID,
+					"HWRevision", &efi_varsiz, efi_var);
+	if (status != EFI_SUCCESS)
+		return status;
+
+	efi_var[efi_varsiz] = '\0';
+	hwrev = atou(efi_var);
+
+	efi_varsiz = sizeof(efi_var) - 1;
+	status = efi_get_variable_early(sys_table_arg, &EFI_AVM_VARIABLE_GUID,
+					"HWSubRevision", &efi_varsiz,
+					efi_var);
+	if (status != EFI_SUCCESS)
+		return status;
+
+	efi_var[efi_varsiz] = '\0';
+	hwsubrev = atou(efi_var);
+
+	status = efi_open_volume(sys_table_arg, image, (void **)&fh);
+	if (status != EFI_SUCCESS)
+		return status;
+
+	for (;;) {
+		status = avm_build_dtb_name_for(hwrev, hwsubrev, filename_16,
+						ARRAY_SIZE(filename_16));
+		if (status != EFI_SUCCESS)
+			return status;
+		if (efi_file_exists(fh, filename_16))
+			break;
+		if (hwsubrev-- == 0) {
+			efi_printk(sys_table_arg, "No DTB found\n");
+			return EFI_NOT_FOUND;
+		}
+	}
+
+	efi_printk(sys_table_arg, "DTB found: ");
+	efi_char16_printk(sys_table_arg, filename_16);
+	efi_printk(sys_table_arg, "\n");
+
+	status = efi_file_size(sys_table_arg, fh, filename_16,
+			       (void **)&dtb_file.handle, &dtb_file.size);
+	if (status != EFI_SUCCESS)
+		return status;
+
+
+	status = efi_high_alloc(sys_table_arg, dtb_file.size +
+				sizeof(struct setup_data), 0x1000,
+				(unsigned long *)&setup_data, ULONG_MAX);
+	if (status != EFI_SUCCESS) {
+		pr_efi_err(sys_table_arg, "Failed to alloc highmem for setup_data\n");
+		goto close_handle;
+	}
+
+	data = setup_data->data;
+	size = dtb_file.size;
+	while (size) {
+		unsigned long chunksize;
+
+		if (IS_ENABLED(CONFIG_X86) && size > __chunk_size)
+			chunksize = __chunk_size;
+		else
+			chunksize = size;
+
+		status = efi_file_read(dtb_file.handle, &chunksize,
+				       data);
+		if (status != EFI_SUCCESS) {
+			pr_efi_err(sys_table_arg, "Failed to read dtb file\n");
+			goto free_setup_data;
+		}
+		data += chunksize;
+		size -= chunksize;
+	}
+
+	efi_file_close(dtb_file.handle);
+
+	setup_data->type = SETUP_DTB;
+	setup_data->len = dtb_file.size;
+	*dtb_setup_data = setup_data;
+
+	return status;
+
+free_setup_data:
+	efi_free(sys_table_arg, dtb_file.size + sizeof(struct setup_data),
+		 (unsigned long)*dtb_setup_data);
+close_handle:
+	efi_file_close(dtb_file.handle);
+	return status;
+}