Add support to patch QCDT

Old Qualcomn devices have their own special QC table of DTB to
store device trees. Since patching fstab is now mandatory on Android 10,
and for older devices all early mount devices have to be included into
the fstab in DTBs, patching QCDT is crucial for rooting Android 10
on legacy devices.

Close #1876 (Thanks for getting me aware of this issue!)

native/jni/magiskboot/dtb.cpp

@@ -7,6 +7,7 @@ extern "C" {
 #include <utils.h>
 #include <bitset>
 #include <vector>
+#include <map>
 
 #include "magiskboot.h"
 #include "format.h"
@@ -16,6 +17,59 @@ using namespace std;
 constexpr int MAX_DEPTH = 32;
 static bitset<MAX_DEPTH> depth_set;
 
+struct qcdt_hdr {
+	char magic[4];          /* "QCDT" */
+	uint32_t version;       /* QCDT version */
+	uint32_t num_dtbs;      /* Number of DTBs */
+} __attribute__((packed));
+
+struct qctable_v1 {
+	uint32_t cpu_info[3];   /* Some CPU info */
+	uint32_t offset;        /* DTB offset in QCDT */
+	uint32_t len;           /* DTB size */
+} __attribute__((packed));
+
+struct qctable_v2 {
+	uint32_t cpu_info[4];   /* Some CPU info */
+	uint32_t offset;        /* DTB offset in QCDT */
+	uint32_t len;           /* DTB size */
+} __attribute__((packed));
+
+struct qctable_v3 {
+	uint32_t cpu_info[8];   /* Some CPU info */
+	uint32_t offset;        /* DTB offset in QCDT */
+	uint32_t len;           /* DTB size */
+} __attribute__((packed));
+
+struct dtb_blob {
+	void *fdt;
+	uint32_t offset;
+	uint32_t len;
+};
+
+template <class Iter>
+class fdt_map_iter {
+public:
+	typedef decltype(std::declval<typename Iter::value_type::second_type>().fdt) value_type;
+	typedef value_type* pointer;
+	typedef value_type& reference;
+
+	explicit fdt_map_iter(Iter j) : i(j) {}
+	fdt_map_iter& operator++() { ++i; return *this; }
+	fdt_map_iter operator++(int) { auto tmp = *this; ++(*this); return tmp; }
+	fdt_map_iter& operator--() { --i; return *this; }
+	fdt_map_iter operator--(int) { auto tmp = *this; --(*this); return tmp; }
+	bool operator==(fdt_map_iter j) const { return i == j.i; }
+	bool operator!=(fdt_map_iter j) const { return !(*this == j); }
+	reference operator*() { return i->second.fdt; }
+	pointer operator->() { return &i->second.fdt; }
+private:
+	Iter i;
+};
+
+template<class Iter>
+inline fdt_map_iter<Iter> make_iter(Iter j) { return fdt_map_iter<Iter>(j); }
+
 static void pretty_node(int depth) {
 	if (depth == 0)
 		return;
@@ -121,33 +175,22 @@ static void dtb_print(const char *file, bool fstab) {
 	}
 	fprintf(stderr, "\n");
 	munmap(dtb, size);
-	exit(0);
 }
 
-static void dtb_patch(const char *in, const char *out) {
+template <typename Iter>
+static bool fdt_patch(Iter first, Iter last) {
 	bool keepverity = check_env("KEEPVERITY");
 	bool redirect = check_env("TWOSTAGEINIT");
-
-	vector<uint8_t *> fdt_list;
-	size_t dtb_sz ;
-	uint8_t *dtb;
-	fprintf(stderr, "Loading dtbs from [%s]\n", in);
-	mmap_ro(in, dtb, dtb_sz);
 	bool modified = false;
-	for (int i = 0; i < dtb_sz; ++i) {
-		if (memcmp(dtb + i, DTB_MAGIC, 4) == 0) {
-			int len = fdt_totalsize(dtb + i);
-			auto fdt = static_cast<uint8_t *>(xmalloc(redirect ? len + 256 : len));
-			memcpy(fdt, dtb + i, len);
-			if (redirect)
-				fdt_open_into(fdt, fdt, len + 256);
-			fdt_list.push_back(fdt);
-			i += len - 1;
 
+	int idx = 0;
+	for (auto it = first; it != last; ++it) {
+		++idx;
+		auto fdt = *it;
 		int fstab = find_fstab(fdt);
 		if (fstab < 0)
 			continue;
-			fprintf(stderr, "Found fstab in dtb.%04d\n", fdt_list.size());
+		fprintf(stderr, "Found fstab in dtb.%04d\n", idx - 1);
 		int block;
 		fdt_for_each_subnode(block, fdt, fstab) {
 			const char *name = fdt_get_name(fdt, block, nullptr);
@@ -169,11 +212,109 @@ static void dtb_patch(const char *in, const char *out) {
 			}
 		}
 	}
+	return modified;
 }
-	munmap(dtb, dtb_sz);
+
-	if (modified) {
+template <class Table>
+static int dtb_patch(const qcdt_hdr *hdr, const char *in, const char *out) {
+	map<uint32_t, dtb_blob> dtb_map;
+	auto buf = reinterpret_cast<const uint8_t *>(hdr);
+	auto tables = reinterpret_cast<const Table *>(hdr + 1);
+
+	// Collect all dtbs
+	for (int i = 0; i < hdr->num_dtbs; ++i) {
+		auto it = dtb_map.find(tables[i].offset);
+		if (it == dtb_map.end()) {
+			auto fdt = xmalloc(tables[i].len + 256);
+			memcpy(fdt, buf + tables[i].offset, tables[i].len);
+			fdt_open_into(fdt, fdt, tables[i].len + 256);
+			dtb_map[tables[i].offset] = { fdt, tables[i].offset };
+		}
+	}
+	if (dtb_map.empty())
+		return 1;
+
+	// Patch fdt
+	if (!fdt_patch(make_iter(dtb_map.begin()), make_iter(dtb_map.end())))
+		return 1;
+
+	if (out == in)
+		unlink(in);
+	int fd = xopen(out, O_RDWR | O_CREAT | O_TRUNC | O_CLOEXEC, 0644);
+
+	// Copy headers and tables
+	xwrite(fd, buf, dtb_map.begin()->first);
+
+	// mmap rw to patch table values retroactively
+	auto mmap_sz = lseek(fd, 0, SEEK_CUR);
+	auto addr = (uint8_t *) xmmap(nullptr, mmap_sz, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
+
+	// Guess page size using gcd
+	auto it = dtb_map.begin();
+	uint32_t page_size = (it++)->first;
+	for (; it != dtb_map.end(); ++it)
+		page_size = binary_gcd(page_size, it->first);
+
+	// Write dtbs
+	for (auto &val : dtb_map) {
+		write_zero(fd, align_off(lseek(fd, 0, SEEK_CUR), page_size));
+		val.second.offset = lseek(fd, 0, SEEK_CUR);
+		auto fdt = val.second.fdt;
+		fdt_pack(fdt);
+		val.second.len = fdt_totalsize(fdt);
+		xwrite(fd, fdt, val.second.len);
+		free(fdt);
+	}
+
+	// Patch tables
+	auto tables_rw = reinterpret_cast<Table *>(addr + sizeof(qcdt_hdr));
+	for (int i = 0; i < hdr->num_dtbs; ++i) {
+		auto &blob = dtb_map[tables_rw[i].offset];
+		tables_rw[i].offset = blob.offset;
+		tables_rw[i].len = blob.len;
+	}
+
+	munmap(addr, mmap_sz);
+	close(fd);
+
+	return 0;
+}
+
+static int dtb_patch(const char *in, const char *out) {
 	if (!out)
 		out = in;
+	size_t dtb_sz ;
+	uint8_t *dtb;
+	fprintf(stderr, "Loading dtbs from [%s]\n", in);
+	mmap_ro(in, dtb, dtb_sz);
+	run_finally f([&]{ munmap(dtb, dtb_sz); });
+
+	if (memcmp(dtb, QCDT_MAGIC, 4) == 0) {
+		auto hdr = reinterpret_cast<qcdt_hdr*>(dtb);
+		switch (hdr->version) {
+			case 1:
+				return dtb_patch<qctable_v1>(hdr, in, out);
+			case 2:
+				return dtb_patch<qctable_v2>(hdr, in, out);
+			case 3:
+				return dtb_patch<qctable_v3>(hdr, in, out);
+			default:
+				return 1;
+		}
+	} else {
+		vector<uint8_t *> fdt_list;
+		for (int i = 0; i < dtb_sz; ++i) {
+			if (memcmp(dtb + i, DTB_MAGIC, 4) == 0) {
+				int len = fdt_totalsize(dtb + i);
+				auto fdt = static_cast<uint8_t *>(xmalloc(len + 256));
+				memcpy(fdt, dtb + i, len);
+				fdt_open_into(fdt, fdt, len + 256);
+				fdt_list.push_back(fdt);
+				i += len - 1;
+			}
+		}
+		if (!fdt_patch(fdt_list.begin(), fdt_list.end()))
+			return 1;
 		int fd = xopen(out, O_WRONLY | O_CREAT | O_TRUNC | O_CLOEXEC, 0644);
 		for (auto fdt : fdt_list) {
 			fdt_pack(fdt);
@@ -182,7 +323,7 @@ static void dtb_patch(const char *in, const char *out) {
 		}
 		close(fd);
 	}
-	exit(!modified);
+	return 0;
 }
 
 int dtb_commands(int argc, char *argv[]) {
@@ -192,11 +333,12 @@ int dtb_commands(int argc, char *argv[]) {
 
 	if (argv[0] == "print"sv) {
 		dtb_print(dtb, argc > 1 && argv[1] == "-f"sv);
+		return 0;
 	} else if (argv[0] == "patch"sv) {
-		dtb_patch(dtb, argv[1]);
+		if (dtb_patch(dtb, argv[1]))
+			exit(1);
+		return 0;
 	} else {
 		return 1;
 	}
-
-	return 0;
 }

native/jni/magiskboot/format.h

@@ -41,6 +41,7 @@ typedef enum {
 #define DTB_MAGIC       "\xd0\x0d\xfe\xed"
 #define LG_BUMP_MAGIC   "\x41\xa9\xe4\x67\x74\x4d\x1d\x1b\xa4\x29\xf2\xec\xea\x65\x52\x79"
 #define DHTB_MAGIC      "\x44\x48\x54\x42\x01\x00\x00\x00"
+#define QCDT_MAGIC      "QCDT"
 #define SEANDROID_MAGIC "SEANDROIDENFORCE"
 #define TEGRABLOB_MAGIC "-SIGNED-BY-SIGNBLOB-"
 #define NOOKHD_RL_MAGIC "Red Loader"

native/jni/utils/misc.cpp

@@ -181,3 +181,20 @@ int parse_int(const char *s) {
 	}
 	return val;
 }
+
+uint32_t binary_gcd(uint32_t u, uint32_t v) {
+	if (u == 0) return v;
+	if (v == 0) return u;
+	auto shift = __builtin_ctz(u | v);
+	u >>= __builtin_ctz(u);
+	do {
+		v >>= __builtin_ctz(v);
+		if (u > v) {
+			auto t = v;
+			v = u;
+			u = t;
+		}
+		v -= u;
+	} while (v != 0);
+	return u << shift;
+}

native/jni/utils/misc.h

@@ -14,6 +14,7 @@ char *rtrim(char *str);
 void init_argv0(int argc, char **argv);
 void set_nice_name(const char *name);
 int parse_int(const char *s);
+uint32_t binary_gcd(uint32_t u, uint32_t v);
 
 #ifdef __cplusplus
 }