diff --git a/util/cbfstool/cbfstool.c b/util/cbfstool/cbfstool.c
index 77dbf72e97..1f71906ef6 100644
--- a/util/cbfstool/cbfstool.c
+++ b/util/cbfstool/cbfstool.c
@@ -27,6 +27,7 @@
 #include "cbfs.h"
 #include "cbfs_image.h"
 #include "cbfs_sections.h"
+#include "elfparsing.h"
 #include "fit.h"
 #include "partitioned_file.h"
 #include <commonlib/fsp.h>
@@ -147,7 +148,8 @@ static unsigned convert_to_from_top_aligned(const struct buffer *region,
 	return convert_to_from_absolute_top_aligned(region, offset);
 }
 
-static int do_cbfs_locate(int32_t *cbfs_addr, size_t metadata_size)
+static int do_cbfs_locate(int32_t *cbfs_addr, size_t metadata_size,
+			size_t data_size)
 {
 	if (!param.filename) {
 		ERROR("You need to specify -f/--filename.\n");
@@ -167,12 +169,18 @@ static int do_cbfs_locate(int32_t *cbfs_addr, size_t metadata_size)
 	if (cbfs_get_entry(&image, param.name))
 		WARN("'%s' already in CBFS.\n", param.name);
 
-	struct buffer buffer;
-	if (buffer_from_file(&buffer, param.filename) != 0) {
-		ERROR("Cannot load %s.\n", param.filename);
-		return 1;
+	if (!data_size) {
+		struct buffer buffer;
+		if (buffer_from_file(&buffer, param.filename) != 0) {
+			ERROR("Cannot load %s.\n", param.filename);
+			return 1;
+		}
+		data_size = buffer.size;
+		buffer_delete(&buffer);
 	}
 
+	DEBUG("File size is %zd (0x%zx)\n", data_size, data_size);
+
 	/* Include cbfs_file size along with space for with name. */
 	metadata_size += cbfs_calculate_file_header_size(param.name);
 	/* Adjust metadata_size if additional attributes were added */
@@ -187,9 +195,8 @@ static int do_cbfs_locate(int32_t *cbfs_addr, size_t metadata_size)
 	if (param.hash != VB2_HASH_INVALID)
 		metadata_size += sizeof(struct cbfs_file_attr_hash);
 
-	int32_t address = cbfs_locate_entry(&image, buffer.size, param.pagesize,
+	int32_t address = cbfs_locate_entry(&image, data_size, param.pagesize,
 						param.alignment, metadata_size);
-	buffer_delete(&buffer);
 
 	if (address == -1) {
 		ERROR("'%s' can't fit in CBFS for page-size %#x, align %#x.\n",
@@ -579,8 +586,15 @@ static int cbfstool_convert_mkstage(struct buffer *buffer, uint32_t *offset,
 
 	if (param.stage_xip) {
 		int32_t address;
+		size_t data_size;
 
-		if (do_cbfs_locate(&address, sizeof(struct cbfs_stage)))  {
+		if (elf_program_file_size(buffer, &data_size) < 0) {
+			ERROR("Could not obtain ELF size\n");
+			return 1;
+		}
+
+		if (do_cbfs_locate(&address, sizeof(struct cbfs_stage),
+			data_size))  {
 			ERROR("Could not find location for XIP stage.\n");
 			return 1;
 		}
@@ -684,7 +698,7 @@ static int cbfs_add(void)
 	if (param.alignment) {
 		/* CBFS compression file attribute is unconditionally added. */
 		size_t metadata_sz = sizeof(struct cbfs_file_attr_compression);
-		if (do_cbfs_locate(&address, metadata_sz))
+		if (do_cbfs_locate(&address, metadata_sz, 0))
 			return 1;
 		local_baseaddress = address;
 	}
diff --git a/util/cbfstool/elfheaders.c b/util/cbfstool/elfheaders.c
index acb25a7475..36226639d0 100644
--- a/util/cbfstool/elfheaders.c
+++ b/util/cbfstool/elfheaders.c
@@ -1440,3 +1440,26 @@ int elf_writer_add_rel(struct elf_writer *ew, const char *sym, Elf64_Addr addr)
 
 	return add_rel(rel_sec, &rel);
 }
+
+int elf_program_file_size(const struct buffer *input, size_t *file_size)
+{
+	Elf64_Ehdr ehdr;
+	Elf64_Phdr *phdr;
+	int i;
+	size_t loadable_file_size = 0;
+
+	if (elf_headers(input, &ehdr, &phdr, NULL))
+		return -1;
+
+	for (i = 0; i < ehdr.e_phnum; i++) {
+		if (phdr[i].p_type != PT_LOAD)
+			continue;
+		loadable_file_size += phdr[i].p_filesz;
+	}
+
+	*file_size = loadable_file_size;
+
+	free(phdr);
+
+	return 0;
+}
diff --git a/util/cbfstool/elfparsing.h b/util/cbfstool/elfparsing.h
index 978592bc46..1c6bf5e81a 100644
--- a/util/cbfstool/elfparsing.h
+++ b/util/cbfstool/elfparsing.h
@@ -124,4 +124,10 @@ int elf_writer_add_rel(struct elf_writer *ew, const char *sym, Elf64_Addr addr);
  */
 int elf_writer_serialize(struct elf_writer *ew, struct buffer *out);
 
+/*
+ * Calculate the loadable program's file size footprint. Returns < 0 on error,
+ * 0 on success.
+ */
+int elf_program_file_size(const struct buffer *input, size_t *file_size);
+
 #endif /* ELFPARSING_H */