+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;
+}
+