msm: dev_tree: add support for multiple concatenated dev treesqcom-dima-8x74-fixes

The kernel build can suppor appending multiple device trees
after the zImage. We now iterate through the device trees
looking for a match according to
platform_id/hardware_id/soc_version in the device tree's /qcom,msm_id
node.

Signed-off-by: Dima Zavin <dima@android.com>

3 files changed, 94 insertions, 14 deletions

diff --git a/app/aboot/aboot.c b/app/aboot/aboot.c
index d95806ca..4759c8f6 100644
--- a/app/aboot/aboot.c
+++ b/app/aboot/aboot.c
@@ -655,6 +655,7 @@ int boot_linux_from_mmc(void)
 			 */
 			void *dtb;
 			dtb = dev_tree_appended((void*) hdr->kernel_addr,
+						kernel_actual,
 						(void *)hdr->tags_addr);
 			if (!dtb) {
 				dprintf(CRITICAL, "ERROR: Appended Device Tree Blob not found\n");
@@ -752,6 +753,7 @@ int boot_linux_from_mmc(void)
 			 */
 			void *dtb;
 			dtb = dev_tree_appended((void*) hdr->kernel_addr,
+						kernel_actual,
 						(void *)hdr->tags_addr);
 			if (!dtb) {
 				dprintf(CRITICAL, "ERROR: Appended Device Tree Blob not found\n");
@@ -1285,7 +1287,8 @@ void cmd_boot(const char *arg, void *data, unsigned sz)
 	 */
 	if (!dtb_copied) {
 		void *dtb;
-		dtb = dev_tree_appended((void *)hdr->kernel_addr, (void *)hdr->tags_addr);
+		dtb = dev_tree_appended((void *)hdr->kernel_addr, kernel_actual,
+					(void *)hdr->tags_addr);
 		if (!dtb) {
 			fastboot_fail("dtb not found");
 			return;
diff --git a/platform/msm_shared/dev_tree.c b/platform/msm_shared/dev_tree.c
index 773c81b9..6b1915a6 100644
--- a/platform/msm_shared/dev_tree.c
+++ b/platform/msm_shared/dev_tree.c
@@ -36,9 +36,71 @@
 #include <platform.h>
 #include <board.h>
 
+#define min(a,b) ((a) < (b) ? (a) : (b))
+
 extern int target_is_emmc_boot(void);
 extern uint32_t target_dev_tree_mem(void *fdt, uint32_t memory_node_offset);
 
+struct msm_id
+{
+	uint32_t platform_id;
+	uint32_t hardware_id;
+	uint32_t soc_rev;
+};
+
+static bool is_dev_tree_compatible(void *dtb)
+{
+	int root_offset;
+	const void *prop;
+	char model[128];
+	struct msm_id msm_id;
+	int len;
+
+	root_offset = fdt_path_offset(dtb, "/");
+	if (root_offset < 0)
+		return false;
+
+	prop = fdt_getprop(dtb, root_offset, "model", &len);
+	if (prop && len > 0) {
+		memcpy(model, prop, min((int)sizeof(model), len));
+		model[sizeof(model) - 1] = '\0';
+	} else {
+		model[0] = '\0';
+	}
+
+	prop = fdt_getprop(dtb, root_offset, "qcom,msm-id", &len);
+	if (!prop || len <= 0) {
+		dprintf(INFO, "qcom,msm-id entry not found\n");
+		return false;
+	} else if (len < (int)sizeof(struct msm_id)) {
+		dprintf(INFO, "qcom,msm-id entry size mismatch (%d != %d)\n",
+			len, sizeof(struct msm_id));
+		return false;
+	}
+	msm_id.platform_id = fdt32_to_cpu(((const struct msm_id *)prop)->platform_id);
+	msm_id.hardware_id = fdt32_to_cpu(((const struct msm_id *)prop)->hardware_id);
+	msm_id.soc_rev = fdt32_to_cpu(((const struct msm_id *)prop)->soc_rev);
+
+	dprintf(INFO, "Found an appended flattened device tree (%s - %d %d 0x%x)\n",
+		*model ? model : "unknown",
+		msm_id.platform_id, msm_id.hardware_id, msm_id.soc_rev);
+
+	if (msm_id.platform_id != board_platform_id() ||
+	    msm_id.hardware_id != board_hardware_id() ||
+	    msm_id.soc_rev != board_soc_version()) {
+		dprintf(INFO, "Device tree's msm_id doesn't match the board: <%d %d 0x%x> != <%d %d 0x%x>\n",
+			msm_id.platform_id,
+			msm_id.hardware_id,
+			msm_id.soc_rev,
+			board_platform_id(),
+			board_hardware_id(),
+			board_soc_version());
+		return false;
+	}
+
+	return true;
+}
+
 /*
  * Will relocate the DTB to the tags addr if the device tree is found and return
  * its address
@@ -48,26 +110,41 @@ extern uint32_t target_dev_tree_mem(void *fdt, uint32_t memory_node_offset);
  * Return Value: DTB address : If appended device tree is found
  *               'NULL'         : Otherwise
  */
-void *dev_tree_appended(void *kernel, void *tags)
+void *dev_tree_appended(void *kernel, uint32_t kernel_size, void *tags)
 {
+	void *kernel_end = kernel + kernel_size;
 	uint32_t app_dtb_offset = 0;
-	uint32_t dtb_magic = 0;
+	void *dtb;
 
 	memcpy((void*) &app_dtb_offset, (void*) (kernel + DTB_OFFSET), sizeof(uint32_t));
-	memcpy((void*) &dtb_magic, (void*) (kernel + app_dtb_offset), sizeof(uint32_t));
-
-	if (dtb_magic == DTB_MAGIC) {
-		void *dtb;
-		int rc;
 
-		dprintf(INFO, "Found Appeneded Flattened Device tree\n");
-		dtb = kernel + app_dtb_offset;
-		rc = fdt_open_into(dtb, tags, fdt_totalsize(dtb));
-		if (rc == 0) {
+	dtb = kernel + app_dtb_offset;
+	while (dtb + sizeof(struct fdt_header) < kernel_end) {
+		bool compat;
+		struct fdt_header dtb_hdr;
+		uint32_t dtb_size;
+
+		/* the DTB could be unaligned, so extract the header,
+		 * and operate on it separately */
+		memcpy(&dtb_hdr, dtb, sizeof(struct fdt_header));
+		if (fdt_check_header((const void *)&dtb_hdr) != 0 ||
+		    (dtb + fdt_totalsize((const void *)&dtb_hdr) > kernel_end))
+			break;
+		dtb_size = fdt_totalsize(&dtb_hdr);
+
+		/* now that we know we have a valid DTB, we need to copy
+		 * it somewhere aligned, like tags */
+		memcpy(tags, dtb, dtb_size);
+
+		compat = is_dev_tree_compatible(tags);
+		if (compat) {
 			/* clear out the old DTB magic so kernel doesn't find it */
-			*((uint32_t *)dtb) = 0;
+			*((uint32_t *)(kernel + app_dtb_offset)) = 0;
 			return tags;
 		}
+
+		/* goto the next device tree if any */
+		dtb += dtb_size;
 	}
 
 	return NULL;
diff --git a/platform/msm_shared/include/dev_tree.h b/platform/msm_shared/include/dev_tree.h
index a27fafde..afc1b05c 100644
--- a/platform/msm_shared/include/dev_tree.h
+++ b/platform/msm_shared/include/dev_tree.h
@@ -67,5 +67,5 @@ enum dt_err_codes
 struct dt_entry * dev_tree_get_entry_ptr(struct dt_table *);
 int update_device_tree(void *, const char *, void *, unsigned);
 int dev_tree_add_mem_info(void *fdt, uint32_t offset, uint32_t size, uint32_t addr);
-void *dev_tree_appended(void *kernel, void *tags);
+void *dev_tree_appended(void *kernel, uint32_t kernel_size, void *tags);
 #endif