1 files changed, 206 insertions, 56 deletions

diff --git a/sys/boot/fdt/fdt_loader_cmd.c b/sys/boot/fdt/fdt_loader_cmd.c
index b52d92f..c7297f4 100644
--- a/sys/boot/fdt/fdt_loader_cmd.c
+++ b/sys/boot/fdt/fdt_loader_cmd.c
@@ -40,8 +40,6 @@ __FBSDID("$FreeBSD$");
 #include "bootstrap.h"
 #include "glue.h"
 
-#define DEBUG
-
 #ifdef DEBUG
 #define debugf(fmt, args...) do { printf("%s(): ", __func__);	\
     printf(fmt,##args); } while (0)
@@ -62,6 +60,8 @@ __FBSDID("$FreeBSD$");
 
 #define FDT_STATIC_DTB_SYMBOL	"fdt_static_dtb"
 
+#define	CMD_REQUIRES_BLOB	0x01
+
 /* Local copy of FDT */
 static struct fdt_header *fdtp = NULL;
 /* Size of FDT blob */
@@ -69,8 +69,11 @@ static size_t fdtp_size = 0;
 /* Location of FDT in kernel or module */
 static vm_offset_t fdtp_va = 0;
 
+static int fdt_load_dtb(vm_offset_t va);
+
 static int fdt_cmd_nyi(int argc, char *argv[]);
 
+static int fdt_cmd_addr(int argc, char *argv[]);
 static int fdt_cmd_mkprop(int argc, char *argv[]);
 static int fdt_cmd_cd(int argc, char *argv[]);
 static int fdt_cmd_hdr(int argc, char *argv[]);
@@ -79,25 +82,28 @@ static int fdt_cmd_prop(int argc, char *argv[]);
 static int fdt_cmd_pwd(int argc, char *argv[]);
 static int fdt_cmd_rm(int argc, char *argv[]);
 static int fdt_cmd_mknode(int argc, char *argv[]);
+static int fdt_cmd_mres(int argc, char *argv[]);
 
 typedef int cmdf_t(int, char *[]);
 
 struct cmdtab {
 	char	*name;
 	cmdf_t	*handler;
+	int	flags;
 };
 
 static const struct cmdtab commands[] = {
-	{ "alias", &fdt_cmd_nyi },
-	{ "cd", &fdt_cmd_cd },
-	{ "header", &fdt_cmd_hdr },
-	{ "ls", &fdt_cmd_ls },
-	{ "mknode", &fdt_cmd_mknode },
-	{ "mkprop", &fdt_cmd_mkprop },
-	{ "mres", &fdt_cmd_nyi },
-	{ "prop", &fdt_cmd_prop },
-	{ "pwd", &fdt_cmd_pwd },
-	{ "rm", &fdt_cmd_rm },
+	{ "addr", &fdt_cmd_addr,	0 },
+	{ "alias", &fdt_cmd_nyi,	0 },
+	{ "cd", &fdt_cmd_cd,		CMD_REQUIRES_BLOB },
+	{ "header", &fdt_cmd_hdr,	CMD_REQUIRES_BLOB },
+	{ "ls", &fdt_cmd_ls,		CMD_REQUIRES_BLOB },
+	{ "mknode", &fdt_cmd_mknode,	CMD_REQUIRES_BLOB },
+	{ "mkprop", &fdt_cmd_mkprop,	CMD_REQUIRES_BLOB },
+	{ "mres", &fdt_cmd_mres,	CMD_REQUIRES_BLOB },
+	{ "prop", &fdt_cmd_prop,	CMD_REQUIRES_BLOB },
+	{ "pwd", &fdt_cmd_pwd,		CMD_REQUIRES_BLOB },
+	{ "rm", &fdt_cmd_rm,		CMD_REQUIRES_BLOB },
 	{ NULL, NULL }
 };
 
@@ -128,7 +134,7 @@ fdt_find_static_dtb()
 	if (md == NULL)
 		return (0);
 	bcopy(md->md_data, &esym, sizeof(esym));
-	// esym is already offset
+	/* esym is already offset */
 
 	md = file_findmetadata(kfp, MODINFOMD_DYNAMIC);
 	if (md == NULL)
@@ -188,53 +194,67 @@ fdt_find_static_dtb()
 }
 
 static int
-fdt_setup_fdtp()
+fdt_load_dtb(vm_offset_t va)
 {
 	struct fdt_header header;
-	struct preloaded_file *bfp;
 	int err;
 
+	COPYOUT(va, &header, sizeof(header));
+	err = fdt_check_header(&header);
+	if (err < 0) {
+		if (err == -FDT_ERR_BADVERSION)
+			sprintf(command_errbuf,
+			    "incompatible blob version: %d, should be: %d",
+			    fdt_version(fdtp), FDT_LAST_SUPPORTED_VERSION);
+
+		else
+			sprintf(command_errbuf, "error validating blob: %s",
+			    fdt_strerror(err));
+		return (1);
+	}
+
 	/*
-	 * Find the device tree blob.
+	 * Release previous blob
 	 */
-	bfp = file_findfile(NULL, "dtb");
-	if (bfp == NULL) {
-		if ((fdtp_va = fdt_find_static_dtb()) == 0) {
-			command_errmsg = "no device tree blob found!";
-			printf("%s\n", command_errmsg);
-			return (CMD_ERROR);
-		}
-	} else {
-		/* Dynamic blob has precedence over static. */
-		fdtp_va = bfp->f_addr;
-	}
+	if (fdtp)
+		free(fdtp);
 
-	COPYOUT(fdtp_va, &header, sizeof(header));
 	fdtp_size = fdt_totalsize(&header);
 	fdtp = malloc(fdtp_size);
+
 	if (fdtp == NULL) {
 		command_errmsg = "can't allocate memory for device tree copy";
-			printf("%s\n", command_errmsg);
-		return (CMD_ERROR);
+		return (1);
 	}
-	COPYOUT(fdtp_va, fdtp, fdtp_size);
 
-	/*
-	 * Validate the blob.
-	 */
-	err = fdt_check_header(fdtp);
-	if (err < 0) {
-		if (err == -FDT_ERR_BADVERSION)
-			sprintf(command_errbuf,
-			    "incompatible blob version: %d, should be: %d",
-			    fdt_version(fdtp), FDT_LAST_SUPPORTED_VERSION);
+	fdtp_va = va;
+	COPYOUT(va, fdtp, fdtp_size);
+	debugf("DTB blob found at 0x%jx, size: 0x%jx\n", (uintmax_t)va, (uintmax_t)fdtp_size);
 
-		else
-			sprintf(command_errbuf, "error validating blob: %s",
-			    fdt_strerror(err));
-		return (CMD_ERROR);
+	return (0);
+}
+
+static int
+fdt_setup_fdtp()
+{
+	struct preloaded_file *bfp;
+	vm_offset_t va;
+
+	bfp = file_findfile(NULL, "dtb");
+	if (bfp == NULL) {
+		if ((va = fdt_find_static_dtb()) == 0) {
+			command_errmsg = "no device tree blob found!";
+			return (1);
+		}
+	} else {
+		/* Dynamic blob has precedence over static. */
+		va = bfp->f_addr;
 	}
-	return (CMD_OK);
+
+	if (fdt_load_dtb(va) != 0)
+		return (1);
+	
+	return (0);
 }
 
 #define fdt_strtovect(str, cellbuf, lim, cellsize) _fdt_strtovect((str), \
@@ -414,6 +434,8 @@ fixup_memory(struct sys_info *si)
 	uint32_t *addr_cellsp, *reg,  *size_cellsp;
 	int err, i, len, memory, realmrno, root;
 	uint8_t *buf, *sb;
+	uint64_t rstart, rsize;
+	int reserved;
 
 	root = fdt_path_offset(fdtp, "/");
 	if (root < 0) {
@@ -453,6 +475,52 @@ fixup_memory(struct sys_info *si)
 	addr_cells = fdt32_to_cpu(*addr_cellsp);
 	size_cells = fdt32_to_cpu(*size_cellsp);
 
+	/*
+	 * Convert memreserve data to memreserve property
+	 * Check if property already exists
+	 */
+	reserved = fdt_num_mem_rsv(fdtp);
+	if (reserved &&
+	    (fdt_getprop(fdtp, root, "memreserve", NULL) == NULL)) {
+		len = (addr_cells + size_cells) * reserved * sizeof(uint32_t);
+		sb = buf = (uint8_t *)malloc(len);
+		if (!buf)
+			return;
+
+		bzero(buf, len);
+
+		for (i = 0; i < reserved; i++) {
+			curmr = &si->mr[i];
+			if (fdt_get_mem_rsv(fdtp, i, &rstart, &rsize))
+				break;
+			if (rsize) {
+				/* Ensure endianess, and put cells into a buffer */
+				if (addr_cells == 2)
+					*(uint64_t *)buf =
+					    cpu_to_fdt64(rstart);
+				else
+					*(uint32_t *)buf =
+					    cpu_to_fdt32(rstart);
+
+				buf += sizeof(uint32_t) * addr_cells;
+				if (size_cells == 2)
+					*(uint64_t *)buf =
+					    cpu_to_fdt64(rsize);
+				else
+					*(uint32_t *)buf =
+					    cpu_to_fdt32(rsize);
+
+				buf += sizeof(uint32_t) * size_cells;
+			}
+		}
+
+		/* Set property */
+		if ((err = fdt_setprop(fdtp, root, "memreserve", sb, len)) < 0)
+			printf("Could not fixup 'memreserve' property.\n");
+
+		free(sb);
+	} 
+
 	/* Count valid memory regions entries in sysinfo. */
 	realmrno = si->mr_no;
 	for (i = 0; i < si->mr_no; i++)
@@ -509,6 +577,8 @@ fixup_memory(struct sys_info *si)
 	/* Set property */
 	if ((err = fdt_setprop(fdtp, memory, "reg", sb, len)) < 0)
 		sprintf(command_errbuf, "Could not fixup '/memory' node.\n");
+
+	free(sb);
 }
 
 void
@@ -564,7 +634,7 @@ fixup_stdout(const char *env)
 /*
  * Locate the blob, fix it up and return its location.
  */
-vm_offset_t
+static vm_offset_t
 fdt_fixup(void)
 {
 	const char *env;
@@ -577,10 +647,12 @@ fdt_fixup(void)
 	ethstr = NULL;
 	len = 0;
 
-	err = fdt_setup_fdtp();
-	if (err) {
-		sprintf(command_errbuf, "No valid device tree blob found!");
-		return (0);
+	if (fdtp == NULL) {
+		err = fdt_setup_fdtp();
+		if (err) {
+			sprintf(command_errbuf, "No valid device tree blob found!");
+			return (0);
+		}
 	}
 
 	/* Create /chosen node (if not exists) */
@@ -640,10 +712,36 @@ success:
 	return (fdtp_va);
 }
 
+/*
+ * Copy DTB blob to specified location and its return size
+ */
+int
+fdt_copy(vm_offset_t va)
+{
+	int err;
+
+	if (fdtp == NULL) {
+		err = fdt_setup_fdtp();
+		if (err) {
+			printf("No valid device tree blob found!");
+			return (0);
+		}
+	}
+
+	if (fdt_fixup() == 0)
+		return (0);
+
+	COPYIN(fdtp, va, fdtp_size);
+	return (fdtp_size);
+}
+
+
+
 int
 command_fdt_internal(int argc, char *argv[])
 {
 	cmdf_t *cmdh;
+	int flags;
 	char *cmd;
 	int i, err;
 
@@ -653,12 +751,6 @@ command_fdt_internal(int argc, char *argv[])
 	}
 
 	/*
-	 * Check if uboot env vars were parsed already. If not, do it now.
-	 */
-	if (fdt_fixup() == 0)
-		return (CMD_ERROR);
-
-	/*
 	 * Validate fdt <command>.
 	 */
 	cmd = strdup(argv[1]);
@@ -668,6 +760,7 @@ command_fdt_internal(int argc, char *argv[])
 		if (strcmp(cmd, commands[i].name) == 0) {
 			/* found it */
 			cmdh = commands[i].handler;
+			flags = commands[i].flags;
 			break;
 		}
 		i++;
@@ -677,6 +770,14 @@ command_fdt_internal(int argc, char *argv[])
 		return (CMD_ERROR);
 	}
 
+	if (flags & CMD_REQUIRES_BLOB) {
+		/*
+		 * Check if uboot env vars were parsed already. If not, do it now.
+		 */
+		if (fdt_fixup() == 0)
+			return (CMD_ERROR);
+	}
+
 	/*
 	 * Call command handler.
 	 */
@@ -686,6 +787,31 @@ command_fdt_internal(int argc, char *argv[])
 }
 
 static int
+fdt_cmd_addr(int argc, char *argv[])
+{
+	vm_offset_t va;
+	char *addr, *cp;
+
+	if (argc > 2)
+		addr = argv[2];
+	else {
+		sprintf(command_errbuf, "no address specified");
+		return (CMD_ERROR);
+	}
+
+	va = strtol(addr, &cp, 0);
+	if (cp == addr) {
+		sprintf(command_errbuf, "Invalid address: %s", addr);
+		return (CMD_ERROR);
+	}
+
+	if (fdt_load_dtb(va) != 0)
+		return (CMD_ERROR);
+
+	return (CMD_OK);
+}
+
+static int
 fdt_cmd_cd(int argc, char *argv[])
 {
 	char *path;
@@ -1445,6 +1571,30 @@ fdt_cmd_pwd(int argc, char *argv[])
 }
 
 static int
+fdt_cmd_mres(int argc, char *argv[])
+{
+	uint64_t start, size;
+	int i, total;
+	char line[80];
+
+	pager_open();
+	total = fdt_num_mem_rsv(fdtp);
+	if (total > 0) {
+		pager_output("Reserved memory regions:\n");
+		for (i = 0; i < total; i++) {
+			fdt_get_mem_rsv(fdtp, i, &start, &size);
+			sprintf(line, "reg#%d: (start: 0x%jx, size: 0x%jx)\n", 
+			    i, start, size);
+			pager_output(line);
+		}
+	} else
+		pager_output("No reserved memory regions\n");
+	pager_close();
+
+	return (CMD_OK);
+}
+
+static int
 fdt_cmd_nyi(int argc, char *argv[])
 {