Magisk/native/jni/magiskboot/dtb.cpp

397 lines
9.9 KiB
C++
Raw Normal View History

2017-09-14 20:52:27 +02:00
#include <unistd.h>
#include <sys/mman.h>
#include <bitset>
#include <vector>
#include <map>
2019-02-25 05:09:34 +01:00
2018-10-25 03:08:06 +02:00
extern "C" {
#include <libfdt.h>
}
2019-02-10 09:57:51 +01:00
#include <utils.h>
2017-09-14 20:52:27 +02:00
#include "magiskboot.h"
#include "dtb.h"
2017-09-14 20:52:27 +02:00
2019-09-20 09:05:14 +02:00
using namespace std;
struct fdt_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); }
2019-10-09 05:49:21 +02:00
constexpr int MAX_DEPTH = 32;
static bitset<MAX_DEPTH> depth_set;
2019-09-20 09:05:14 +02:00
static void pretty_node(int depth) {
if (depth == 0)
return;
for (int i = 0; i < depth - 1; ++i)
printf(depth_set[i] ? "│ " : " ");
printf(depth_set[depth - 1] ? "├── " : "└── ");
2019-09-20 09:05:14 +02:00
}
static void pretty_prop(int depth) {
for (int i = 0; i < depth; ++i)
printf(depth_set[i] ? "│ " : " ");
printf(depth_set[depth] ? "│ " : " ");
2019-09-20 09:05:14 +02:00
}
static void print_node(const void *fdt, int node = 0, int depth = 0) {
// Print node itself
pretty_node(depth);
printf("#%d: %s\n", node, fdt_get_name(fdt, node, nullptr));
// Print properties
depth_set[depth] = fdt_first_subnode(fdt, node) >= 0;
2017-11-09 20:45:06 +01:00
int prop;
fdt_for_each_property_offset(prop, fdt, node) {
2019-09-20 09:05:14 +02:00
pretty_prop(depth);
2017-11-09 20:45:06 +01:00
int size;
const char *name;
2019-09-20 09:05:14 +02:00
auto value = static_cast<const char *>(fdt_getprop_by_offset(fdt, prop, &name, &size));
bool is_str = !(size > 1 && value[0] == 0);
if (is_str) {
// Scan through value to see if printable
for (int i = 0; i < size; ++i) {
char c = value[i];
if (i == size - 1) {
// Make sure null terminate
is_str = c == '\0';
} else if ((c > 0 && c < 32) || c >= 127) {
is_str = false;
break;
}
}
}
if (is_str) {
printf("[%s]: [%s]\n", name, value);
} else {
printf("[%s]: <bytes>(%d)\n", name, size);
}
2017-11-09 20:45:06 +01:00
}
2019-09-20 09:05:14 +02:00
// Recursive
if (depth_set[depth]) {
int child;
int prev = -1;
fdt_for_each_subnode(child, fdt, node) {
if (prev >= 0)
print_node(fdt, prev, depth + 1);
prev = child;
}
depth_set[depth] = false;
print_node(fdt, prev, depth + 1);
2017-10-07 16:08:10 +02:00
}
}
2017-09-14 20:52:27 +02:00
static int find_fstab(const void *fdt, int node = 0) {
if (fdt_get_name(fdt, node, nullptr) == "fstab"sv)
return node;
int child;
fdt_for_each_subnode(child, fdt, node) {
int fstab = find_fstab(fdt, child);
if (fstab >= 0)
2017-09-14 20:52:27 +02:00
return fstab;
}
return -1;
}
2019-09-20 09:53:58 +02:00
static void dtb_print(const char *file, bool fstab) {
size_t size;
uint8_t *dtb;
2017-10-07 16:08:10 +02:00
fprintf(stderr, "Loading dtbs from [%s]\n", file);
2019-02-25 05:09:34 +01:00
mmap_ro(file, dtb, size);
2017-10-07 16:08:10 +02:00
// Loop through all the dtbs
int dtb_num = 0;
for (int i = 0; i < size; ++i) {
if (memcmp(dtb + i, FDT_MAGIC_STR, 4) == 0) {
auto fdt = dtb + i;
2019-09-20 09:53:58 +02:00
if (fstab) {
int node = find_fstab(fdt);
if (node >= 0) {
fprintf(stderr, "Found fstab in dtb.%04d\n", dtb_num);
print_node(fdt, node);
}
} else {
fprintf(stderr, "Printing dtb.%04d\n", dtb_num);
print_node(fdt);
}
++dtb_num;
2019-10-05 23:24:53 +02:00
i += fdt_totalsize(fdt) - 1;
2017-10-07 16:08:10 +02:00
}
}
fprintf(stderr, "\n");
munmap(dtb, size);
}
template <typename Iter>
static bool fdt_patch(Iter first, Iter last) {
bool keepverity = check_env("KEEPVERITY");
2019-09-22 12:45:23 +02:00
bool redirect = check_env("TWOSTAGEINIT");
bool modified = false;
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<const char *>(
fdt_getprop(fdt, block, "fsmgr_flags", reinterpret_cast<int *>(&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 <class Table, class Header>
static int dtb_patch(const Header *hdr, const char *in, const char *out) {
map<uint32_t, fdt_blob> dtb_map;
auto buf = reinterpret_cast<const uint8_t *>(hdr);
auto tables = reinterpret_cast<const Table *>(hdr + 1);
constexpr bool is_dt_table = std::is_same_v<Header, dt_table_header>;
using endian_conv = uint32_t (*)(uint32_t);
endian_conv be_to_le;
endian_conv le_to_be;
if constexpr (is_dt_table) {
be_to_le = fdt32_to_cpu;
le_to_be = cpu_to_fdt32;
} else {
be_to_le = le_to_be = [](uint32_t x) -> auto { return x; };
}
// Collect all dtbs
auto num_dtb = be_to_le(hdr->num_dtbs);
for (int i = 0; i < num_dtb; ++i) {
auto offset = be_to_le(tables[i].offset);
if (dtb_map.count(offset) == 0) {
auto blob = buf + offset;
uint32_t size = fdt_totalsize(blob);
2019-10-08 04:57:01 +02:00
auto fdt = xmalloc(size + 256);
memcpy(fdt, blob, size);
fdt_open_into(fdt, fdt, size + 256);
dtb_map[offset] = { fdt, 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);
uint32_t total_size = 0;
// Copy headers and tables
total_size += 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 alignment using gcd
uint32_t align = 1;
if constexpr (!is_dt_table) {
auto it = dtb_map.begin();
align = (it++)->first;
for (; it != dtb_map.end(); ++it)
align = binary_gcd(align, it->first);
}
// Write dtbs
for (auto &val : dtb_map) {
val.second.offset = lseek(fd, 0, SEEK_CUR);
auto fdt = val.second.fdt;
fdt_pack(fdt);
int size = fdt_totalsize(fdt);
total_size += xwrite(fd, fdt, size);
val.second.len = do_align(size, align);
write_zero(fd, align_off(lseek(fd, 0, SEEK_CUR), align));
// total_size += align_off(lseek(fd, 0, SEEK_CUR), align); /* Not needed */
free(fdt);
}
// Patch headers
if constexpr (is_dt_table) {
auto hdr_rw = reinterpret_cast<Header *>(addr);
hdr_rw->total_size = le_to_be(total_size);
}
2019-10-09 05:49:21 +02:00
auto tables_rw = reinterpret_cast<Table *>(addr + sizeof(Header));
for (int i = 0; i < num_dtb; ++i) {
auto &blob = dtb_map[be_to_le(tables_rw[i].offset)];
tables_rw[i].offset = le_to_be(blob.offset);
tables_rw[i].len = le_to_be(blob.len);
}
munmap(addr, mmap_sz);
close(fd);
return 0;
}
2019-10-09 05:49:21 +02:00
#define MATCH(s) (memcmp(dtb, s, sizeof(s) - 1) == 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); });
2019-10-09 05:49:21 +02:00
if (MATCH(QCDT_MAGIC)) {
auto hdr = reinterpret_cast<qcdt_hdr*>(dtb);
switch (hdr->version) {
case 1:
fprintf(stderr, "QCDT v1\n");
return dtb_patch<qctable_v1>(hdr, in, out);
case 2:
fprintf(stderr, "QCDT v2\n");
return dtb_patch<qctable_v2>(hdr, in, out);
case 3:
fprintf(stderr, "QCDT v3\n");
return dtb_patch<qctable_v3>(hdr, in, out);
default:
return 1;
}
2019-10-09 05:49:21 +02:00
} else if (MATCH(DTBH_MAGIC)) {
auto hdr = reinterpret_cast<dtbh_hdr *>(dtb);
switch (hdr->version) {
case 2:
fprintf(stderr, "DTBH v2\n");
return dtb_patch<bhtable_v2>(hdr, in, out);
default:
return 1;
}
2019-10-09 05:49:21 +02:00
} else if (MATCH(PXADT_MAGIC)) {
auto hdr = reinterpret_cast<pxadt_hdr *>(dtb);
switch (hdr->version) {
case 1:
fprintf(stderr, "PXA-DT v1\n");
return dtb_patch<pxatable_v1>(hdr, in, out);
2019-10-09 05:49:21 +02:00
default:
return 1;
}
} else if (MATCH(PXA19xx_MAGIC)) {
auto hdr = reinterpret_cast<pxa19xx_hdr *>(dtb);
2019-10-09 05:49:21 +02:00
switch (hdr->version) {
case 1:
fprintf(stderr, "PXA-19xx v1\n");
return dtb_patch<pxatable_v1>(hdr, in, out);
default:
return 1;
}
} else if (MATCH(SPRD_MAGIC)) {
auto hdr = reinterpret_cast<sprd_hdr *>(dtb);
switch (hdr->version) {
case 1:
fprintf(stderr, "SPRD v1\n");
return dtb_patch<sprdtable_v1>(hdr, in, out);
2019-10-09 05:49:21 +02:00
default:
return 1;
}
} else if (MATCH(DT_TABLE_MAGIC)) {
auto hdr = reinterpret_cast<dt_table_header *>(dtb);
switch (hdr->version) {
case 0:
fprintf(stderr, "DT_TABLE v0\n");
return dtb_patch<dt_table_entry>(hdr, in, out);
default:
return 1;
}
} else {
vector<uint8_t *> fdt_list;
for (int i = 0; i < dtb_sz; ++i) {
if (memcmp(dtb + i, FDT_MAGIC_STR, 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;
2017-09-14 20:52:27 +02:00
}
}
if (!fdt_patch(fdt_list.begin(), fdt_list.end()))
return 1;
2019-10-05 23:24:53 +02:00
int fd = xopen(out, O_WRONLY | O_CREAT | O_TRUNC | O_CLOEXEC, 0644);
for (auto fdt : fdt_list) {
fdt_pack(fdt);
xwrite(fd, fdt, fdt_totalsize(fdt));
2019-10-05 23:24:53 +02:00
free(fdt);
}
close(fd);
}
return 0;
2017-11-10 18:30:33 +01:00
}
2019-09-20 09:53:58 +02:00
int dtb_commands(int argc, char *argv[]) {
char *dtb = argv[0];
++argv;
--argc;
if (argv[0] == "print"sv) {
dtb_print(dtb, argc > 1 && argv[1] == "-f"sv);
return 0;
2019-09-20 09:53:58 +02:00
} else if (argv[0] == "patch"sv) {
if (dtb_patch(dtb, argv[1]))
exit(1);
return 0;
2019-09-20 09:53:58 +02:00
} else {
return 1;
}
2017-09-14 20:52:27 +02:00
}