libfdt: Handle v16 and re-ordered trees for r/w

Currently all the read/write functions in libfdt require that the
given tree be v17, and further, that the tree has the memory
reservation block, structure block and strings block stored in that
physical order.

This patch eases these constraints, by making fdt_open_int() reorder
the blocks, and/or convert the tree to v17, so that it will then be
ready for the other read-write functions.

It also extends fdt_pack() to actually remove any gaps between blocks
that might be present.

Signed-off-by: David Gibson <david@gibson.dropbear.id.au>

libfdt/fdt_rw.c

@@ -55,6 +55,18 @@
 
 #include "libfdt_internal.h"
 
+static int _blocks_misordered(const void *fdt,
+			      int mem_rsv_size, int struct_size)
+{
+	return (fdt_off_mem_rsvmap(fdt) < ALIGN(sizeof(struct fdt_header), 8))
+		|| (fdt_off_dt_struct(fdt) <
+		    (fdt_off_mem_rsvmap(fdt) + mem_rsv_size))
+		|| (fdt_off_dt_strings(fdt) <
+		    (fdt_off_dt_struct(fdt) + struct_size))
+		|| (fdt_totalsize(fdt) <
+		    (fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt)));
+}
+
 static int rw_check_header(void *fdt)
 {
 	int err;
@@ -63,16 +75,8 @@ static int rw_check_header(void *fdt)
 		return err;
 	if (fdt_version(fdt) < 17)
 		return -FDT_ERR_BADVERSION;
-	if (fdt_off_mem_rsvmap(fdt) < ALIGN(sizeof(struct fdt_header), 8))
-		return -FDT_ERR_BADLAYOUT;
-	if (fdt_off_dt_struct(fdt) <
-	    (fdt_off_mem_rsvmap(fdt) + sizeof(struct fdt_reserve_entry)))
-		return -FDT_ERR_BADLAYOUT;
-	if (fdt_off_dt_strings(fdt) <
-	    (fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt)))
-		return -FDT_ERR_BADLAYOUT;
-	if (fdt_totalsize(fdt) <
-	    (fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt)))
+	if (_blocks_misordered(fdt, sizeof(struct fdt_reserve_entry),
+			       fdt_size_dt_struct(fdt)))
 		return -FDT_ERR_BADLAYOUT;
 	if (fdt_version(fdt) > 17)
 		fdt_set_version(fdt, 17);
@@ -342,36 +346,102 @@ int fdt_del_node(void *fdt, int nodeoffset)
 				   endoffset - nodeoffset, 0);
 }
 
-int fdt_open_into(void *fdt, void *buf, int bufsize)
+static void _packblocks(const void *fdt, void *buf,
+		       int mem_rsv_size, int struct_size)
+{
+	int mem_rsv_off, struct_off, strings_off;
+
+	mem_rsv_off = ALIGN(sizeof(struct fdt_header), 8);
+	struct_off = mem_rsv_off + mem_rsv_size;
+	strings_off = struct_off + struct_size;
+
+	memmove(buf + mem_rsv_off, fdt + fdt_off_mem_rsvmap(fdt), mem_rsv_size);
+	fdt_set_off_mem_rsvmap(buf, mem_rsv_off);
+
+	memcpy(buf + struct_off, fdt + fdt_off_dt_struct(fdt), struct_size);
+	fdt_set_off_dt_struct(buf, struct_off);
+	fdt_set_size_dt_struct(buf, struct_size);
+
+	memcpy(buf + strings_off, fdt + fdt_off_dt_strings(fdt),
+	       fdt_size_dt_strings(fdt));
+	fdt_set_off_dt_strings(buf, strings_off);
+	fdt_set_size_dt_strings(buf, fdt_size_dt_strings(fdt));
+}
+
+int fdt_open_into(const void *fdt, void *buf, int bufsize)
 {
 	int err;
+	int mem_rsv_size, struct_size;
+	int newsize;
+	void *tmp;
 
-	err = fdt_move(fdt, buf, bufsize);
+	err = fdt_check_header(fdt);
 	if (err)
 		return err;
 
-	fdt = buf;
+	mem_rsv_size = (fdt_num_mem_rsv(fdt)+1)
+		* sizeof(struct fdt_reserve_entry);
 
-	fdt_set_totalsize(fdt, bufsize);
+	if (fdt_version(fdt) >= 17) {
+		struct_size = fdt_size_dt_struct(fdt);
+	} else {
+		struct_size = 0;
+		while (fdt_next_tag(fdt, struct_size, &struct_size) != FDT_END)
+			;
+	}
 
-	/* FIXME: re-order if necessary */
+	if (!_blocks_misordered(fdt, mem_rsv_size, struct_size)) {
+		/* no further work necessary */
+		err = fdt_move(fdt, buf, bufsize);
+		if (err)
+			return err;
+		fdt_set_version(buf, 17);
+		fdt_set_size_dt_struct(buf, struct_size);
+		fdt_set_totalsize(buf, bufsize);
+		return 0;
+	}
 
-	err = rw_check_header(fdt);
-	if (err)
-		return err;
+	/* Need to reorder */
+	newsize = ALIGN(sizeof(struct fdt_header), 8) + mem_rsv_size
+		+ struct_size + fdt_size_dt_strings(fdt);
+
+	if (bufsize < newsize)
+		return -FDT_ERR_NOSPACE;
+
+	if (((buf + newsize) <= fdt)
+	    || (buf >= (fdt + fdt_totalsize(fdt)))) {
+		tmp = buf;
+	} else {
+		tmp = (void *)fdt + fdt_totalsize(fdt);
+		if ((tmp + newsize) > (buf + bufsize))
+			return -FDT_ERR_NOSPACE;
+	}
+
+	_packblocks(fdt, tmp, mem_rsv_size, struct_size);
+	memmove(buf, tmp, newsize);
+
+	fdt_set_magic(buf, FDT_MAGIC);
+	fdt_set_totalsize(buf, bufsize);
+	fdt_set_version(buf, 17);
+	fdt_set_last_comp_version(buf, 16);
+	fdt_set_boot_cpuid_phys(buf, fdt_boot_cpuid_phys(fdt));
 
 	return 0;
 }
 
 int fdt_pack(void *fdt)
 {
+	int mem_rsv_size;
 	int err;
 
 	err = rw_check_header(fdt);
 	if (err)
 		return err;
 
-	/* FIXME: pack components */
+	mem_rsv_size = (fdt_num_mem_rsv(fdt)+1)
+		* sizeof(struct fdt_reserve_entry);
+	_packblocks(fdt, fdt, mem_rsv_size, fdt_size_dt_struct(fdt));
 	fdt_set_totalsize(fdt, _blob_data_size(fdt));
+
 	return 0;
 }

libfdt/libfdt.h

@@ -412,7 +412,7 @@ int fdt_finish(void *fdt);
 /* Read-write functions                                               */
 /**********************************************************************/
 
-int fdt_open_into(void *fdt, void *buf, int bufsize);
+int fdt_open_into(const void *fdt, void *buf, int bufsize);
 int fdt_pack(void *fdt);
 
 int fdt_add_mem_rsv(void *fdt, uint64_t address, uint64_t size);