1 files changed, 198 insertions, 40 deletions

diff --git a/tools/ifdtool.c b/tools/ifdtool.c
index df16616..1f95203 100644
--- a/tools/ifdtool.c
+++ b/tools/ifdtool.c
@@ -706,10 +706,11 @@ int inject_region(char *image, int size, int region_type, char *region_fname)
  *			0xffffffff so use an address relative to that. For an
  *			8MB ROM the start address is 0xfff80000.
  * @write_fname:	Filename to add to the image
+ * @offset_uboot_top:	Offset of the top of U-Boot
  * @return number of bytes written if OK, -ve on error
  */
 static int write_data(char *image, int size, unsigned int addr,
-		      const char *write_fname)
+		      const char *write_fname, int offset_uboot_top)
 {
 	int write_fd, write_size;
 	int offset;
@@ -719,6 +720,14 @@ static int write_data(char *image, int size, unsigned int addr,
 		return write_fd;
 
 	offset = (uint32_t)(addr + size);
+	if (offset_uboot_top && offset_uboot_top >= offset) {
+		fprintf(stderr, "U-Boot image overlaps with region '%s'\n",
+			write_fname);
+		fprintf(stderr,
+			"U-Boot finishes at offset %x, file starts at %x\n",
+			offset_uboot_top, offset);
+		return -EXDEV;
+	}
 	debug("Writing %s to offset %#x\n", write_fname, offset);
 
 	if (offset < 0 || offset + write_size > size) {
@@ -737,6 +746,171 @@ static int write_data(char *image, int size, unsigned int addr,
 	return write_size;
 }
 
+static int scan_ucode(const void *blob, char *ucode_base, int *countp,
+		      const char **datap, int *data_sizep)
+{
+	const char *data = NULL;
+	int node, count;
+	int data_size;
+	char *ucode;
+
+	for (node = 0, count = 0, ucode = ucode_base; node >= 0; count++) {
+		node = fdt_node_offset_by_compatible(blob, node,
+						     "intel,microcode");
+		if (node < 0)
+			break;
+
+		data = fdt_getprop(blob, node, "data", &data_size);
+		if (!data) {
+			debug("Missing microcode data in FDT '%s': %s\n",
+			      fdt_get_name(blob, node, NULL),
+			      fdt_strerror(data_size));
+			return -ENOENT;
+		}
+
+		if (ucode_base)
+			memcpy(ucode, data, data_size);
+		ucode += data_size;
+	}
+
+	if (countp)
+		*countp = count;
+	if (datap)
+		*datap = data;
+	if (data_sizep)
+		*data_sizep = data_size;
+
+	return ucode - ucode_base;
+}
+
+static int remove_ucode(char *blob)
+{
+	int node, count;
+	int ret;
+
+	/* Keep going until we find no more microcode to remove */
+	do {
+		for (node = 0, count = 0; node >= 0;) {
+			int ret;
+
+			node = fdt_node_offset_by_compatible(blob, node,
+							     "intel,microcode");
+			if (node < 0)
+				break;
+
+			ret = fdt_delprop(blob, node, "data");
+
+			/*
+			 * -FDT_ERR_NOTFOUND means we already removed the
+			 * data for this one, so we just continue.
+			 * 0 means we did remove it, so offsets may have
+			 * changed and we need to restart our scan.
+			 * Anything else indicates an error we should report.
+			 */
+			if (ret == -FDT_ERR_NOTFOUND)
+				continue;
+			else if (!ret)
+				node = 0;
+			else
+				return ret;
+		}
+	} while (count);
+
+	/* Pack down to remove excees space */
+	ret = fdt_pack(blob);
+	if (ret)
+		return ret;
+
+	return fdt_totalsize(blob);
+}
+
+static int write_ucode(char *image, int size, struct input_file *fdt,
+		       int fdt_size, unsigned int ucode_ptr,
+		       int collate_ucode)
+{
+	const char *data = NULL;
+	char *ucode_buf;
+	const void *blob;
+	char *ucode_base;
+	uint32_t *ptr;
+	int ucode_size;
+	int data_size;
+	int offset;
+	int count;
+	int ret;
+
+	blob = (void *)image + (uint32_t)(fdt->addr + size);
+
+	debug("DTB at %lx\n", (char *)blob - image);
+
+	/* Find out about the micrcode we have */
+	ucode_size = scan_ucode(blob, NULL, &count, &data, &data_size);
+	if (ucode_size < 0)
+		return ucode_size;
+	if (!count) {
+		debug("No microcode found in FDT\n");
+		return -ENOENT;
+	}
+
+	if (count > 1 && !collate_ucode) {
+		fprintf(stderr,
+			"Cannot handle multiple microcode blocks - please use -C flag to collate them\n");
+		return -EMLINK;
+	}
+
+	/*
+	 * Collect the microcode into a buffer, remove it from the device
+	 * tree and place it immediately above the (now smaller) device tree.
+	 */
+	if (collate_ucode && count > 1) {
+		ucode_buf = malloc(ucode_size);
+		if (!ucode_buf) {
+			fprintf(stderr,
+				"Out of memory for microcode (%d bytes)\n",
+				ucode_size);
+			return -ENOMEM;
+		}
+		ret = scan_ucode(blob, ucode_buf, NULL, NULL, NULL);
+		if (ret < 0)
+			return ret;
+
+		/* Remove the microcode from the device tree */
+		ret = remove_ucode((char *)blob);
+		if (ret < 0) {
+			debug("Could not remove FDT microcode: %s\n",
+			      fdt_strerror(ret));
+			return -EINVAL;
+		}
+		debug("Collated %d microcode block(s)\n", count);
+		debug("Device tree reduced from %x to %x bytes\n",
+		      fdt_size, ret);
+		fdt_size = ret;
+
+		/*
+		 * Place microcode area immediately above the FDT, aligned
+		 * to a 16-byte boundary.
+		 */
+		ucode_base = (char *)(((unsigned long)blob + fdt_size + 15) &
+				~15);
+
+		data = ucode_base;
+		data_size = ucode_size;
+		memcpy(ucode_base, ucode_buf, ucode_size);
+		free(ucode_buf);
+	}
+
+	offset = (uint32_t)(ucode_ptr + size);
+	ptr = (void *)image + offset;
+
+	ptr[0] = (data - image) - size;
+	ptr[1] = data_size;
+	debug("Wrote microcode pointer at %x: addr=%x, size=%x\n", ucode_ptr,
+	      ptr[0], ptr[1]);
+
+	return (collate_ucode ? data + data_size : (char *)blob + fdt_size) -
+			image;
+}
+
 /**
  * write_uboot() - Write U-Boot, device tree and microcode pointer
  *
@@ -752,51 +926,28 @@ static int write_data(char *image, int size, unsigned int addr,
  * @return 0 if OK, -ve on error
  */
 static int write_uboot(char *image, int size, struct input_file *uboot,
-		       struct input_file *fdt, unsigned int ucode_ptr)
+		       struct input_file *fdt, unsigned int ucode_ptr,
+		       int collate_ucode)
 {
 	const void *blob;
-	const char *data;
-	int uboot_size;
-	uint32_t *ptr;
-	int data_size;
-	int offset;
-	int node;
-	int ret;
+	int uboot_size, fdt_size;
 
-	uboot_size = write_data(image, size, uboot->addr, uboot->fname);
+	uboot_size = write_data(image, size, uboot->addr, uboot->fname, 0);
 	if (uboot_size < 0)
 		return uboot_size;
 	fdt->addr = uboot->addr + uboot_size;
 	debug("U-Boot size %#x, FDT at %#x\n", uboot_size, fdt->addr);
-	ret = write_data(image, size, fdt->addr, fdt->fname);
-	if (ret < 0)
-		return ret;
+	fdt_size = write_data(image, size, fdt->addr, fdt->fname, 0);
+	if (fdt_size < 0)
+		return fdt_size;
+	blob = (void *)image + (uint32_t)(fdt->addr + size);
 
 	if (ucode_ptr) {
-		blob = (void *)image + (uint32_t)(fdt->addr + size);
-		debug("DTB at %lx\n", (char *)blob - image);
-		node = fdt_node_offset_by_compatible(blob, 0,
-						     "intel,microcode");
-		if (node < 0) {
-			debug("No microcode found in FDT: %s\n",
-			      fdt_strerror(node));
-			return -ENOENT;
-		}
-		data = fdt_getprop(blob, node, "data", &data_size);
-		if (!data) {
-			debug("No microcode data found in FDT: %s\n",
-			      fdt_strerror(data_size));
-			return -ENOENT;
-		}
-		offset = (uint32_t)(ucode_ptr + size);
-		ptr = (void *)image + offset;
-		ptr[0] = (data - image) - size;
-		ptr[1] = data_size;
-		debug("Wrote microcode pointer at %x: addr=%x, size=%x\n",
-		      ucode_ptr, ptr[0], ptr[1]);
+		return write_ucode(image, size, fdt, fdt_size, ucode_ptr,
+				   collate_ucode);
 	}
 
-	return 0;
+	return ((char *)blob + fdt_size) - image;
 }
 
 static void print_version(void)
@@ -860,7 +1011,7 @@ int main(int argc, char *argv[])
 	int mode_dump = 0, mode_extract = 0, mode_inject = 0;
 	int mode_spifreq = 0, mode_em100 = 0, mode_locked = 0;
 	int mode_unlocked = 0, mode_write = 0, mode_write_descriptor = 0;
-	int create = 0;
+	int create = 0, collate_ucode = 0;
 	char *region_type_string = NULL, *inject_fname = NULL;
 	char *desc_fname = NULL, *addr_str = NULL;
 	int region_type = -1, inputfreq = 0;
@@ -880,6 +1031,7 @@ int main(int argc, char *argv[])
 	int ret;
 	static struct option long_options[] = {
 		{"create", 0, NULL, 'c'},
+		{"collate-microcode", 0, NULL, 'C'},
 		{"dump", 0, NULL, 'd'},
 		{"descriptor", 1, NULL, 'D'},
 		{"em100", 0, NULL, 'e'},
@@ -898,12 +1050,15 @@ int main(int argc, char *argv[])
 		{0, 0, 0, 0}
 	};
 
-	while ((opt = getopt_long(argc, argv, "cdD:ef:hi:lm:r:s:uU:vw:x?",
+	while ((opt = getopt_long(argc, argv, "cCdD:ef:hi:lm:r:s:uU:vw:x?",
 				  long_options, &option_index)) != EOF) {
 		switch (opt) {
 		case 'c':
 			create = 1;
 			break;
+		case 'C':
+			collate_ucode = 1;
+			break;
 		case 'd':
 			mode_dump = 1;
 			break;
@@ -1113,22 +1268,25 @@ int main(int argc, char *argv[])
 	}
 
 	if (mode_write_descriptor)
-		ret = write_data(image, size, -size, desc_fname);
+		ret = write_data(image, size, -size, desc_fname, 0);
 
 	if (mode_inject)
 		ret = inject_region(image, size, region_type, inject_fname);
 
 	if (mode_write) {
+		int offset_uboot_top = 0;
+
 		for (wr_idx = 0; wr_idx < wr_num; wr_idx++) {
 			ifile = &input_file[wr_idx];
 			if (ifile->type == IF_fdt) {
 				continue;
 			} else if (ifile->type == IF_uboot) {
 				ret = write_uboot(image, size, ifile, fdt,
-						  ucode_ptr);
+						  ucode_ptr, collate_ucode);
+				offset_uboot_top = ret;
 			} else {
 				ret = write_data(image, size, ifile->addr,
-					 ifile->fname);
+					 ifile->fname, offset_uboot_top);
 			}
 			if (ret < 0)
 				break;