3 files changed, 90 insertions, 2 deletions

diff --git a/util/cbfstool/cbfstool.c b/util/cbfstool/cbfstool.c
index bf8b3812b8..d8f2ac1bb3 100644
--- a/util/cbfstool/cbfstool.c
+++ b/util/cbfstool/cbfstool.c
@@ -28,6 +28,7 @@ typedef enum {
 	CMD_ADD_PAYLOAD,
 	CMD_ADD_STAGE,
 	CMD_CREATE,
+	CMD_LOCATE,
 	CMD_PRINT
 } cmd_t;
 
@@ -78,8 +79,7 @@ static int cbfs_add(int argc, char **argv)
 	if (argc > 6) {
 		base = strtoul(argv[6], NULL, 0);
 	}
-	cbfsfile =
-	    create_cbfs_file(cbfsname, filedata, &filesize, type, &base);
+	cbfsfile = create_cbfs_file(cbfsname, filedata, &filesize, type, &base);
 	if (add_file_to_cbfs(cbfsfile, filesize, base))
 		return 1;
 	writerom(romname, rom, romsize);
@@ -203,6 +203,23 @@ static int cbfs_create(int argc, char **argv)
 	return create_cbfs_image(romname, size, bootblock, align);
 }
 
+static int cbfs_locate(int argc, char **argv)
+{
+	char *romname = argv[1];
+	if (argc < 6) {
+		printf("not enough arguments to 'locate'.\n");
+		return 1;
+	}
+
+	const char *file = argv[3];
+	uint32_t filesize = getfilesize(file);
+	const char *filename = argv[4];
+	int align = strtoul(argv[5], NULL, 0);
+
+	printf("%x\n", cbfs_find_location(romname, filesize, filename, align));
+	return 0;
+}
+
 static int cbfs_print(int argc, char **argv)
 {
 	char *romname = argv[1];
@@ -223,6 +240,7 @@ struct command commands[] = {
 	{CMD_ADD_PAYLOAD, "add-payload", cbfs_add_payload},
 	{CMD_ADD_STAGE, "add-stage", cbfs_add_stage},
 	{CMD_CREATE, "create", cbfs_create},
+	{CMD_LOCATE, "locate", cbfs_locate},
 	{CMD_PRINT, "print", cbfs_print}
 };
 
@@ -238,6 +256,7 @@ void usage(void)
 	     "add-payload FILE NAME [COMP] [base]  Add a payload to the ROM\n"
 	     "add-stage FILE NAME [COMP] [base]    Add a stage to the ROM\n"
 	     "create SIZE BSIZE BOOTBLOCK [ALIGN]  Create a ROM file\n"
+	     "locate FILE NAME ALIGN               Find a place for a file of that size\n"
 	     "print                                Show the contents of the ROM\n");
 }
 
diff --git a/util/cbfstool/common.c b/util/cbfstool/common.c
index 4c453995ed..1f69fa37f5 100644
--- a/util/cbfstool/common.c
+++ b/util/cbfstool/common.c
@@ -27,6 +27,16 @@
 
 #define dprintf
 
+uint32_t getfilesize(const char *filename)
+{
+	uint32_t size;
+	FILE *file = fopen(filename, "rb");
+	fseek(file, 0, SEEK_END);
+	size = ftell(file);
+	fclose(file);
+	return size;
+}
+
 void *loadfile(const char *filename, uint32_t * romsize_p, void *content,
 	       int place)
 {
@@ -323,3 +333,58 @@ int create_cbfs_image(const char *romfile, uint32_t _romsize,
 	writerom(romfile, romarea, romsize);
 	return 0;
 }
+
+static int in_segment(int addr, int size, int gran)
+{
+	return ((addr & ~(gran - 1)) == ((addr + size) & ~(gran - 1)));
+}
+
+uint32_t cbfs_find_location(const char *romfile, uint32_t filesize,
+			    const char *filename, uint32_t alignment)
+{
+	void *rom = loadrom(romfile);
+	int filename_size = strlen(filename);
+
+	int headersize =
+	    sizeof(struct cbfs_file) + ALIGN(filename_size + 1,
+					     16) + sizeof(struct cbfs_stage);
+	int totalsize = headersize + filesize;
+
+	uint32_t current = phys_start;
+	while (current < phys_end) {
+		if (!cbfs_file_header(current)) {
+			current += align;
+			continue;
+		}
+		struct cbfs_file *thisfile =
+		    (struct cbfs_file *)phys_to_virt(current);
+
+		uint32_t top =
+		    current + ntohl(thisfile->len) + ntohl(thisfile->offset);
+		if (((ntohl(thisfile->type) == 0x0)
+		     || (ntohl(thisfile->type) == 0xffffffff))
+		    && (ntohl(thisfile->len) + ntohl(thisfile->offset) >=
+			totalsize)) {
+			if (in_segment
+			    (current + headersize, filesize, alignment))
+				return current + headersize;
+			if ((ALIGN(current, alignment) + filesize < top)
+			    && (ALIGN(current, alignment) - headersize >
+				current)
+			    && in_segment(ALIGN(current, alignment), filesize,
+					  alignment))
+				return ALIGN(current, alignment);
+			if ((ALIGN(current, alignment) + alignment + filesize <
+			     top)
+			    && (ALIGN(current, alignment) + alignment -
+				headersize > current)
+			    && in_segment(ALIGN(current, alignment) + alignment,
+					  filesize, alignment))
+				return ALIGN(current, alignment) + alignment;
+		}
+		current =
+		    ALIGN(current + ntohl(thisfile->len) +
+			  ntohl(thisfile->offset), align);
+	}
+	return 0;
+}
diff --git a/util/cbfstool/common.h b/util/cbfstool/common.h
index a41eb8a439..ede06e5340 100644
--- a/util/cbfstool/common.h
+++ b/util/cbfstool/common.h
@@ -34,6 +34,7 @@ static uint32_t virt_to_phys(void *addr)
 
 #define ALIGN(val, by) (((val) + (by)-1)&~((by)-1))
 
+uint32_t getfilesize(const char *filename);
 void *loadfile(const char *filename, uint32_t * romsize_p, void *content,
 	       int place);
 void *loadrom(const char *filename);
@@ -62,4 +63,7 @@ int create_cbfs_image(const char *romfile, uint32_t romsize,
 int add_file_to_cbfs(void *content, uint32_t contentsize, uint32_t location);
 void print_cbfs_directory(const char *filename);
 
+uint32_t cbfs_find_location(const char *romfile, uint32_t filesize,
+			    const char *filename, uint32_t align);
+
 #define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0]))