[][Add tool to support embed dm-verity bootargs into fdt]
[Description]
Add tool to support embed dm-verity bootargs into fdt
[Release-log]
N/A
Change-Id: I65ef262484ecc48efc513b0ef1a0cb96e5440011
Reviewed-on: https://gerrit.mediatek.inc/c/openwrt/feeds/mtk_openwrt_feeds/+/5850557
diff --git a/tools/fdt-patch-dm-verify/src/main.c b/tools/fdt-patch-dm-verify/src/main.c
new file mode 100644
index 0000000..ccd7094
--- /dev/null
+++ b/tools/fdt-patch-dm-verify/src/main.c
@@ -0,0 +1,685 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2022 MediaTek Inc. All Rights Reserved.
+ *
+ * Author: Weijie Gao <weijie.gao@mediatek.com>
+ *
+ * Tool for modifying bootargs in dtb for dm-verity
+ */
+
+#ifdef _MSC_VER
+#define _CRT_SECURE_NO_WARNINGS
+#define _CRT_NONSTDC_NO_WARNINGS
+#endif /* _MSC_VER */
+
+#define _GNU_SOURCE
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdarg.h>
+#include <malloc.h>
+#include <ctype.h>
+#include <errno.h>
+#include <libfdt.h>
+
+#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
+
+struct kvpair {
+ char *name;
+ char *value;
+ bool quoted_value;
+ bool deleted;
+};
+
+static void *fdt;
+static char *veritysummary;
+static uint32_t fdt_len, summary_len;
+
+static struct kvpair *summary_lines;
+static uint32_t summary_line_count;
+
+static struct kvpair *bootargs_items;
+static uint32_t bootargs_item_count;
+
+#ifdef _MSC_VER
+int vasprintf(char **strp, const char *format, va_list ap)
+{
+ int len = _vscprintf(format, ap);
+ if (len == -1)
+ return -1;
+ char *str = (char *)malloc((size_t)len + 1);
+ if (!str)
+ return -1;
+ int retval = vsnprintf(str, len + 1, format, ap);
+ if (retval == -1) {
+ free(str);
+ return -1;
+ }
+ *strp = str;
+ return retval;
+}
+
+int asprintf(char **strp, const char *format, ...)
+{
+ va_list ap;
+ va_start(ap, format);
+ int retval = vasprintf(strp, format, ap);
+ va_end(ap);
+ return retval;
+}
+
+static char *strndup(const char *str, size_t n)
+{
+ size_t len = strlen(str);
+ char *s;
+
+ if (len < n)
+ return strdup(str);
+
+ s = malloc(n + 1);
+ if (!s)
+ return NULL;
+
+ memcpy(s, str, n);
+ s[n] = 0;
+
+ return s;
+}
+#endif /* _MSC_VER */
+
+static int read_file(const char *file, void **buffer, uint32_t *filelen)
+{
+ size_t rdlen;
+ int ret = 0;
+ uint8_t *ptr;
+ FILE *f;
+ long len;
+
+ f = fopen(file, "rb");
+ if (!f) {
+ fprintf(stderr, "Failed to open file '%s', error %d\n", file, errno);
+ return -errno;
+ }
+
+ ret = fseek(f, 0, SEEK_END);
+ if (ret < 0) {
+ ret = ferror(f);
+ fprintf(stderr, "fseek() failed, error %d\n", ret);
+ goto cleanup;
+ }
+
+ len = ftell(f);
+ if (len < 0) {
+ ret = ferror(f);
+ fprintf(stderr, "ftell() failed, error %d\n", ret);
+ goto cleanup;
+ }
+
+ ret = fseek(f, 0, SEEK_SET);
+ if (ret < 0) {
+ ret = ferror(f);
+ fprintf(stderr, "fseek() failed, error %d\n", ret);
+ goto cleanup;
+ }
+
+ ptr = malloc(len + 1);
+ if (!ptr) {
+ ret = ferror(f);
+ fprintf(stderr, "Failed to allocate memory\n");
+ goto cleanup;
+ }
+
+ rdlen = fread(ptr, 1, len, f);
+ if (rdlen != len) {
+ ret = ferror(f);
+ fprintf(stderr, "Failed to read file, error %d\n", ret);
+ free(ptr);
+ *buffer = NULL;
+ goto cleanup;
+ }
+
+ ptr[len] = 0;
+
+ *buffer = (void *)ptr;
+
+ if (filelen)
+ *filelen = len;
+
+cleanup:
+ fclose(f);
+ return ret;
+}
+
+static int write_file(const char *file, const void *buffer, uint32_t len)
+{
+ size_t wrlen;
+ FILE *f;
+ int ret;
+
+ f = fopen(file, "wb");
+ if (!f) {
+ fprintf(stderr, "Failed to open file '%s', error %d\n", file, errno);
+ return -errno;
+ }
+
+ wrlen = fwrite(buffer, 1, len, f);
+ ret = ferror(f);
+ fclose(f);
+
+ if (wrlen != len) {
+ fprintf(stderr, "Failed to write file, error %d\n", ret);
+ return ret;
+ }
+
+ return 0;
+}
+
+static struct kvpair *kvpair_find(struct kvpair *pairs, uint32_t count, const char *name)
+{
+ uint32_t i;
+
+ for (i = 0; i < count; i++) {
+ if (!strcmp(pairs[i].name, name))
+ return &pairs[i];
+ }
+
+ return NULL;
+}
+
+/* find the first line-ending char in a string */
+static inline char *strcrlf(char *str)
+{
+ while (*str) {
+ if (*str == '\r' || *str == '\n')
+ return str;
+ str++;
+ }
+
+ return NULL;
+}
+
+/* find the first NULL char in a string */
+static inline char *strnull(char *str, size_t len)
+{
+ while (len) {
+ if (!*str)
+ return str;
+ str++;
+ len--;
+ }
+
+ return NULL;
+}
+
+/* trim all whitespace chars at the beginning of a string by moving pointer */
+static inline char *ltrim(const char *str)
+{
+ while (*str == 0x20 || *str == '\t')
+ str++;
+
+ return (char *)str;
+}
+
+/* trim all whitespace chars at the end of a string. chars will be changed */
+static inline void rtrim_len(char *str, size_t len)
+{
+ char *eos = str + len - 1;
+
+ while (eos >= str) {
+ if (*eos != 0x20 && *eos != '\t')
+ break;
+
+ *eos = 0;
+ eos--;
+ }
+}
+
+/* derived from libkvcutil */
+static int parse_verity_summary(void)
+{
+ char *ptr = veritysummary, *pcrlf, *psep, *peol, *pkey;
+ uint32_t lines = 1;
+
+ /*
+ * first step: count total number of lines
+ * this make sure we only allocate memory once
+ */
+ do {
+ pcrlf = strcrlf(ptr);
+ if (!pcrlf)
+ break;
+
+ /*
+ * rules of line splitting:
+ * - CR + LF: treat as one line (Windows/DOS)
+ * - CR + ^LF: Mac
+ * - LF: Unix/Linux
+ */
+ if (pcrlf[0] == '\r' && pcrlf[1] == '\n')
+ ptr = pcrlf + 2;
+ else
+ ptr = pcrlf + 1;
+
+ lines++;
+ } while (1);
+
+ /* allocate memory to store parsed lines */
+ summary_lines = calloc(lines, sizeof(*summary_lines));
+ if (!summary_lines) {
+ fprintf(stderr, "Failed to allocate memory for summary parsing\n");
+ return -ENOMEM;
+ }
+
+ /* second step: split lines and parse keys and values */
+ ptr = veritysummary;
+
+ do {
+ peol = strcrlf(ptr);
+ pcrlf = peol;
+
+ /* split a line and record its line-ending */
+ if (pcrlf) {
+ if (pcrlf[0] == '\r' && pcrlf[1] == '\n') {
+ pcrlf[0] = 0;
+ pcrlf += 2;
+ } else {
+ pcrlf[0] = 0;
+ pcrlf += 1;
+ }
+ } else {
+ /* CR/LF not found. should be the last line */
+ peol = strnull(ptr, summary_len - (ptr - veritysummary) + 1);
+ pcrlf = peol + 1;
+ }
+
+ /* trim leading spaces of key */
+ pkey = ltrim(ptr);
+
+ psep = strchr(pkey, ':');
+
+ if (!psep) {
+ /* line has no ':' */
+ goto next_line;
+ }
+
+ psep[0] = 0;
+
+ /* trim trailing spaces of key */
+ rtrim_len(pkey, psep - pkey);
+
+ /* trim white spaces of value */
+ psep = ltrim(psep + 1);
+ rtrim_len(psep, peol - psep);
+
+ summary_lines[summary_line_count].name = pkey;
+ summary_lines[summary_line_count].value = psep;
+ summary_line_count++;
+
+ next_line:
+ ptr = pcrlf;
+ if (ptr >= veritysummary + summary_len)
+ break;
+ } while (1);
+
+ return 0;
+}
+
+static const char *verity_summary_find(const char *name)
+{
+ struct kvpair *p = kvpair_find(summary_lines, summary_line_count, name);
+
+ if (p)
+ return p->value;
+
+ return NULL;
+}
+
+/**
+ * skip_spaces - Removes leading whitespace from @str.
+ * @str: The string to be stripped.
+ *
+ * Returns a pointer to the first non-whitespace character in @str.
+ */
+char *skip_spaces(const char *str)
+{
+ while (isspace(*str))
+ ++str;
+ return (char *)str;
+}
+
+/* derived from linux kernel */
+static const char *get_arg_next(const char *args, const char **param, size_t *keylen)
+{
+ unsigned int i, equals = 0;
+ int in_quote = 0;
+
+ args = skip_spaces(args);
+ if (!*args)
+ return NULL;
+
+ if (*args == '"') {
+ args++;
+ in_quote = 1;
+ }
+
+ for (i = 0; args[i]; i++) {
+ if (isspace(args[i]) && !in_quote)
+ break;
+
+ if (equals == 0) {
+ if (args[i] == '=')
+ equals = i;
+ }
+
+ if (args[i] == '"')
+ in_quote = !in_quote;
+ }
+
+ *param = args;
+
+ if (equals)
+ *keylen = equals;
+ else
+ *keylen = i;
+
+ return args + i;
+}
+
+static int parse_bootargs(const char *bootargs)
+{
+ const char *n = bootargs, *p;
+ size_t len, keylen;
+ uint32_t i = 0;
+
+ while (1) {
+ n = get_arg_next(n, &p, &keylen);
+ if (!n)
+ break;
+
+ bootargs_item_count++;
+ }
+
+ if (!bootargs_item_count)
+ return 0;
+
+ bootargs_items = calloc(bootargs_item_count, sizeof(*bootargs_items));
+ if (!bootargs_items) {
+ fprintf(stderr, "Failed to allocate memory for summary parsing\n");
+ return -ENOMEM;
+ }
+
+ n = bootargs;
+
+ while (1) {
+ n = get_arg_next(n, &p, &keylen);
+ if (!n)
+ break;
+
+ len = n - p;
+
+ bootargs_items[i].name = strndup(p, keylen);
+ if (!bootargs_items[i].name)
+ return -ENOMEM;
+
+ if (keylen < len) {
+ if (p[keylen + 1] == '\"') {
+ bootargs_items[i].value = strndup(p + keylen + 2, len - keylen - 3);
+ bootargs_items[i].quoted_value = true;
+ } else {
+ bootargs_items[i].value = strndup(p + keylen + 1, len - keylen - 1);
+ }
+
+ if (!bootargs_items[i].value)
+ return -ENOMEM;
+ }
+
+ i++;
+ }
+
+ return 0;
+}
+
+static const char *bootargs_find(const char *name)
+{
+ struct kvpair *p = kvpair_find(bootargs_items, bootargs_item_count, name);
+
+ if (p)
+ return p->value;
+
+ return NULL;
+}
+
+static char *strconcat(char *dst, const char *src)
+{
+ while (*src)
+ *dst++ = *src++;
+
+ return dst;
+}
+
+static char *merge_bootargs(struct kvpair *new_pairs, uint32_t num)
+{
+ struct kvpair *kvp;
+ uint32_t i, len = 0;
+ char *val;
+ char *str, *p;
+ bool quoted;
+
+ /* Estimate new booargs size */
+ for (i = 0; i < bootargs_item_count; i++) {
+ len += strlen(bootargs_items[i].name);
+ if (bootargs_items[i].value)
+ len += strlen(bootargs_items[i].value);
+ len += 4; /* =, "", space */
+ }
+
+ for (i = 0; i < num; i++) {
+ len += strlen(new_pairs[i].name);
+ if (new_pairs[i].value)
+ len += strlen(new_pairs[i].value);
+ len += 4; /* =, "", space */
+ }
+
+ str = calloc(len + 1, 1);
+ if (!str)
+ return NULL;
+
+ /* Merge */
+ p = str;
+
+ /* Existed or overwritten */
+ for (i = 0; i < bootargs_item_count; i++) {
+ kvp = kvpair_find(new_pairs, num, bootargs_items[i].name);
+ if (kvp) {
+ val = kvp->value;
+ quoted = kvp->quoted_value;
+ kvp->deleted = true;
+ } else {
+ val = bootargs_items[i].value;
+ quoted = bootargs_items[i].quoted_value;
+ }
+
+ p = strconcat(p, bootargs_items[i].name);
+ if (val) {
+ *p++ = '=';
+
+ if (quoted)
+ *p++ = '\"';
+
+ p = strconcat(p, val);
+
+ if (quoted)
+ *p++ = '\"';
+ }
+
+ *p++ = ' ';
+ }
+
+ /* New */
+ for (i = 0; i < num; i++) {
+ if (new_pairs[i].deleted)
+ continue;
+
+ p = strconcat(p, new_pairs[i].name);
+ if (new_pairs[i].value) {
+ *p++ = '=';
+
+ if (new_pairs[i].quoted_value)
+ *p++ = '\"';
+
+ p = strconcat(p, new_pairs[i].value);
+
+ if (new_pairs[i].quoted_value)
+ *p++ = '\"';
+ }
+
+ *p++ = ' ';
+ }
+
+ p[-1] = 0;
+
+ return str;
+}
+
+int main(int argc, char *argv[])
+{
+ const char *datablocks, *datablock_size, *hashblock_size, *hash_algo, *salt, *root_hash;
+ const char *bootargs, *rootdev;
+ int ret, nodeoffset, len;
+ struct kvpair dmpairs[2];
+ uint32_t datablocks_num;
+ char *dmstr, *nfdt;
+ size_t nlen;
+
+ if (argc < 4) {
+ printf("Usage: <summary> <fdt-in> <fdt-out> [override-root]\n");
+ return 0;
+ }
+
+ ret = read_file(argv[1], (void **)&veritysummary, &summary_len);
+ if (ret)
+ return 1;
+
+ ret = read_file(argv[2], &fdt, &fdt_len);
+ if (ret)
+ return 2;
+
+ if (parse_verity_summary())
+ return 3;
+
+ /* find "/chosen" node. */
+ nodeoffset = fdt_subnode_offset(fdt, 0, "chosen");
+ if (nodeoffset < 0) {
+ fprintf(stderr, "Node `chosen' not found\n");
+ return 4;
+ }
+
+ bootargs = fdt_getprop(fdt, nodeoffset, "bootargs", &len);
+ if (!bootargs) {
+ fprintf(stderr, "Property `bootargs' not found\n");
+ return 5;
+ }
+
+ parse_bootargs(bootargs);
+
+ /* find rootdev */
+ rootdev = bootargs_find("root");
+ if (!rootdev) {
+ if (argc == 4 || !strlen(argv[4])) {
+ fprintf(stderr, "`root' not found in `bootargs`\n");
+ return 6;
+ }
+
+ if (strchr(argv[4], ' ') || strchr(argv[4], '\t') || strcrlf(argv[4])) {
+ fprintf(stderr, "Overrided `root' must not contain whitespace\n");
+ return 6;
+ }
+
+ rootdev = argv[4];
+ }
+
+ /* No dm-mod.create is expected */
+ if (bootargs_find("dm-mod.create")) {
+ fprintf(stderr, "Found unexpected `dm-mod.create' in `bootargs'\n");
+ return 7;
+ }
+
+ /* Assemble `dm-mod.create' */
+ datablocks = verity_summary_find("Data blocks");
+ datablock_size = verity_summary_find("Data block size");
+ hashblock_size = verity_summary_find("Hash block size");
+ hash_algo = verity_summary_find("Hash algorithm");
+ salt = verity_summary_find("Salt");
+ root_hash = verity_summary_find("Root hash");
+
+ if (!datablocks || !datablock_size || !hashblock_size || !hash_algo || !salt || !root_hash) {
+ fprintf(stderr, "Incomplete summary of veritysetup\n");
+ return 8;
+ }
+
+ datablocks_num = strtoul(datablocks, NULL, 0);
+ if (datablocks_num == ULONG_MAX) {
+ fprintf(stderr, "Data blocks is invalid\n");
+ return 9;
+ }
+
+ ret = asprintf(&dmstr, "dm-verity,,,ro,0 %u verity 1 %s %s %s %s %u %u %s %s %s",
+ datablocks_num * 8, rootdev, rootdev, datablock_size, hashblock_size,
+ datablocks_num, datablocks_num + 1, hash_algo, root_hash, salt);
+ if (ret < 0) {
+ fprintf(stderr, "Failed to format `dm-mod.create'\n");
+ return 9;
+ }
+
+ /* Assemble new bootargs */
+ memset(dmpairs, 0, sizeof(dmpairs));
+
+ dmpairs[0].name = "root";
+ dmpairs[0].value = "/dev/dm-0";
+
+ dmpairs[1].name = "dm-mod.create";
+ dmpairs[1].value = dmstr;
+ dmpairs[1].quoted_value = true;
+
+ bootargs = merge_bootargs(dmpairs, ARRAY_SIZE(dmpairs));
+ if (!bootargs) {
+ fprintf(stderr, "Failed to merge new bootargs\n");
+ return 10;
+ }
+
+ /* Resize dtb buffer */
+ nlen = strlen(bootargs) + 1;
+ nfdt = realloc(fdt, fdt_len + nlen);
+ if (!nfdt) {
+ fprintf(stderr, "Failed to extend fdt buffer\n");
+ return 11;
+ }
+
+ memset((uint8_t *)nfdt + fdt_len, 0, nlen);
+
+ fdt = nfdt;
+
+ /* Modify bootagrs in dtb */
+ ret = fdt_open_into(fdt, fdt, fdt_totalsize(fdt) + nlen);
+ if (ret) {
+ fprintf(stderr, "Failed to extend fdt size\n");
+ return 11;
+ }
+
+ ret = fdt_setprop(fdt, nodeoffset, "bootargs", bootargs, nlen);
+ if (ret < 0) {
+ fprintf(stderr, "Failed to set new `bootargs'\n");
+ return 12;
+ }
+
+ /* Change the fdt header to reflect the correct size */
+ fdt_set_totalsize(fdt, fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt));
+
+ /* Save fdt */
+ ret = write_file(argv[3], fdt, fdt_totalsize(fdt));
+ if (ret)
+ return 13;
+
+ return 0;
+}