diff --git a/native/jni/magiskboot/dtb.cpp b/native/jni/magiskboot/dtb.cpp index 0c2be07fd..e43171e1f 100644 --- a/native/jni/magiskboot/dtb.cpp +++ b/native/jni/magiskboot/dtb.cpp @@ -7,6 +7,7 @@ extern "C" { #include #include #include +#include #include "magiskboot.h" #include "format.h" @@ -16,6 +17,59 @@ using namespace std; constexpr int MAX_DEPTH = 32; static bitset 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 fdt_map_iter { +public: + typedef decltype(std::declval().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 +inline fdt_map_iter make_iter(Iter j) { return fdt_map_iter(j); } + static void pretty_node(int depth) { if (depth == 0) return; @@ -121,59 +175,146 @@ 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 +static bool fdt_patch(Iter first, Iter last) { bool keepverity = check_env("KEEPVERITY"); bool redirect = check_env("TWOSTAGEINIT"); + bool modified = false; - vector fdt_list; + 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", idx - 1); + int block; + fdt_for_each_subnode(block, fdt, fstab) { + const char *name = fdt_get_name(fdt, block, nullptr); + fprintf(stderr, "Found entry [%s] in fstab\n", name); + if (!keepverity) { + uint32_t size; + auto value = static_cast( + fdt_getprop(fdt, block, "fsmgr_flags", reinterpret_cast(&size))); + char *pval = patch_verity(value, size); + if (pval) { + modified = true; + fdt_setprop_string(fdt, block, "fsmgr_flags", pval); + } + } + if (redirect && name == "system"sv) { + modified = true; + fprintf(stderr, "Changing mnt_point to /system_root\n"); + fdt_setprop_string(fdt, block, "mnt_point", "/system_root"); + } + } + } + return modified; +} + +template +static int dtb_patch(const qcdt_hdr *hdr, const char *in, const char *out) { + map dtb_map; + auto buf = reinterpret_cast(hdr); + auto tables = reinterpret_cast(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(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); - 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(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; + run_finally f([&]{ munmap(dtb, dtb_sz); }); - int fstab = find_fstab(fdt); - if (fstab < 0) - continue; - fprintf(stderr, "Found fstab in dtb.%04d\n", fdt_list.size()); - int block; - fdt_for_each_subnode(block, fdt, fstab) { - const char *name = fdt_get_name(fdt, block, nullptr); - fprintf(stderr, "Found entry [%s] in fstab\n", name); - if (!keepverity) { - uint32_t size; - auto value = static_cast( - fdt_getprop(fdt, block, "fsmgr_flags", reinterpret_cast(&size))); - char *pval = patch_verity(value, size); - if (pval) { - modified = true; - fdt_setprop_string(fdt, block, "fsmgr_flags", pval); - } - } - if (redirect && name == "system"sv) { - modified = true; - fprintf(stderr, "Changing mnt_point to /system_root\n"); - fdt_setprop_string(fdt, block, "mnt_point", "/system_root"); - } + if (memcmp(dtb, QCDT_MAGIC, 4) == 0) { + auto hdr = reinterpret_cast(dtb); + switch (hdr->version) { + case 1: + return dtb_patch(hdr, in, out); + case 2: + return dtb_patch(hdr, in, out); + case 3: + return dtb_patch(hdr, in, out); + default: + return 1; + } + } else { + vector 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(xmalloc(len + 256)); + memcpy(fdt, dtb + i, len); + fdt_open_into(fdt, fdt, len + 256); + fdt_list.push_back(fdt); + i += len - 1; } } - } - munmap(dtb, dtb_sz); - if (modified) { - if (!out) - out = in; + 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; } diff --git a/native/jni/magiskboot/format.h b/native/jni/magiskboot/format.h index a445e6200..1552e5d7c 100644 --- a/native/jni/magiskboot/format.h +++ b/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" diff --git a/native/jni/utils/misc.cpp b/native/jni/utils/misc.cpp index 9d680ce5d..b01da66b1 100644 --- a/native/jni/utils/misc.cpp +++ b/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; +} diff --git a/native/jni/utils/misc.h b/native/jni/utils/misc.h index ad6616431..a55516f30 100644 --- a/native/jni/utils/misc.h +++ b/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 }