dev_tree: Fix reading multiple appended DTBs

Sadly they might not be aligned properly. In that case we have to make a copy... This is slow but at least it works. Oh well.
third-tester · Sep 22, 2021 · ca3f92d · ca3f92d
1 parent 1767e62
commit ca3f92d
Showing 1 changed file with 14 additions and 6 deletions.
diff --git a/platform/msm_shared/dev_tree.c b/platform/msm_shared/dev_tree.c
@@ -124,7 +124,8 @@ static void dt_entry_list_delete(struct dt_entry_node *dt_node_member)
 	}
 }
 
-static int dev_tree_compatible(void *dtb, uint32_t dtb_size, int root_offset, struct dt_entry_node *dtb_list)
+static int dev_tree_compatible(void *dtb, void *real_dtb, uint32_t dtb_size,
+			       int root_offset, struct dt_entry_node *dtb_list)
 {
 	const void *prop = NULL;
 	const char *plat_prop = NULL;
@@ -232,7 +233,7 @@ static int dev_tree_compatible(void *dtb, uint32_t dtb_size, int root_offset, st
 			cur_dt_entry->pmic_rev[1] = board_pmic_target(1);
 			cur_dt_entry->pmic_rev[2] = board_pmic_target(2);
 			cur_dt_entry->pmic_rev[3] = board_pmic_target(3);
-			cur_dt_entry->offset = (uint32_t)dtb;
+			cur_dt_entry->offset = (uint32_t)real_dtb;
 			cur_dt_entry->size = dtb_size;
 
 			dprintf(SPEW, "Found an appended flattened device tree (%s - %u %u 0x%x)\n",
@@ -380,7 +381,7 @@ static int dev_tree_compatible(void *dtb, uint32_t dtb_size, int root_offset, st
 						dt_entry_array[k].pmic_rev[1]= pmic_data[n].pmic_version[1];
 						dt_entry_array[k].pmic_rev[2]= pmic_data[n].pmic_version[2];
 						dt_entry_array[k].pmic_rev[3]= pmic_data[n].pmic_version[3];
-						dt_entry_array[k].offset = (uint32_t)dtb;
+						dt_entry_array[k].offset = (uint32_t)real_dtb;
 						dt_entry_array[k].size = dtb_size;
 						k++;
 					}
@@ -394,7 +395,7 @@ static int dev_tree_compatible(void *dtb, uint32_t dtb_size, int root_offset, st
 					dt_entry_array[k].pmic_rev[1]= board_pmic_target(1);
 					dt_entry_array[k].pmic_rev[2]= board_pmic_target(2);
 					dt_entry_array[k].pmic_rev[3]= board_pmic_target(3);
-					dt_entry_array[k].offset = (uint32_t)dtb;
+					dt_entry_array[k].offset = (uint32_t)real_dtb;
 					dt_entry_array[k].size = dtb_size;
 					k++;
 				}
@@ -487,6 +488,7 @@ void *dev_tree_appended(void *kernel, uint32_t kernel_size, uint32_t dtb_offset,
 	dtb = kernel + app_dtb_offset;
 	while (((uintptr_t)dtb + sizeof(struct fdt_header)) < (uintptr_t)kernel_end) {
 		struct fdt_header dtb_hdr __attribute__ ((aligned(8)));
+		void *dtb_aligned = dtb;
 		uint32_t dtb_size;
 
 		/* the DTB could be unaligned, so extract the header,
@@ -504,7 +506,13 @@ void *dev_tree_appended(void *kernel, uint32_t kernel_size, uint32_t dtb_offset,
 			return NULL;
 		}
 
-		dev_tree_compatible(dtb, dtb_size, 0, dt_entry_queue);
+		/* Make sure DTB is aligned properly if necessary :/ */
+		if ((uintptr_t)dtb & 7) {
+			memcpy(tags, dtb, dtb_size);
+			dtb_aligned = tags;
+		}
+
+		dev_tree_compatible(dtb_aligned, dtb, dtb_size, 0, dt_entry_queue);
 
 		/* goto the next device tree if any */
 		dtb += dtb_size;
@@ -1540,7 +1548,7 @@ int dev_tree_get_dt_entry(const void *fdt, int offset, struct dt_entry *dt_entry
 		.dt_entry_m = dt_entry
 	};
 
-	if (!dev_tree_compatible((void*)fdt, fdt_totalsize(fdt), offset, &node))
+	if (!dev_tree_compatible((void*)fdt, (void*)fdt, fdt_totalsize(fdt), offset, &node))
 		return false;
 
 	if (platform_dt_absolute_match(dt_entry, NULL)) {