[][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/libfdt/fdt_sw.c b/tools/fdt-patch-dm-verify/src/libfdt/fdt_sw.c
new file mode 100644
index 0000000..76bea22
--- /dev/null
+++ b/tools/fdt-patch-dm-verify/src/libfdt/fdt_sw.c
@@ -0,0 +1,376 @@
+// SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause)
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ */
+#include "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+static int fdt_sw_probe_(void *fdt)
+{
+ if (fdt_magic(fdt) == FDT_MAGIC)
+ return -FDT_ERR_BADSTATE;
+ else if (fdt_magic(fdt) != FDT_SW_MAGIC)
+ return -FDT_ERR_BADMAGIC;
+ return 0;
+}
+
+#define FDT_SW_PROBE(fdt) \
+ { \
+ int err; \
+ if ((err = fdt_sw_probe_(fdt)) != 0) \
+ return err; \
+ }
+
+/* 'memrsv' state: Initial state after fdt_create()
+ *
+ * Allowed functions:
+ * fdt_add_reservmap_entry()
+ * fdt_finish_reservemap() [moves to 'struct' state]
+ */
+static int fdt_sw_probe_memrsv_(void *fdt)
+{
+ int err = fdt_sw_probe_(fdt);
+ if (err)
+ return err;
+
+ if (fdt_off_dt_strings(fdt) != 0)
+ return -FDT_ERR_BADSTATE;
+ return 0;
+}
+
+#define FDT_SW_PROBE_MEMRSV(fdt) \
+ { \
+ int err; \
+ if ((err = fdt_sw_probe_memrsv_(fdt)) != 0) \
+ return err; \
+ }
+
+/* 'struct' state: Enter this state after fdt_finish_reservemap()
+ *
+ * Allowed functions:
+ * fdt_begin_node()
+ * fdt_end_node()
+ * fdt_property*()
+ * fdt_finish() [moves to 'complete' state]
+ */
+static int fdt_sw_probe_struct_(void *fdt)
+{
+ int err = fdt_sw_probe_(fdt);
+ if (err)
+ return err;
+
+ if (fdt_off_dt_strings(fdt) != fdt_totalsize(fdt))
+ return -FDT_ERR_BADSTATE;
+ return 0;
+}
+
+#define FDT_SW_PROBE_STRUCT(fdt) \
+ { \
+ int err; \
+ if ((err = fdt_sw_probe_struct_(fdt)) != 0) \
+ return err; \
+ }
+
+static inline uint32_t sw_flags(void *fdt)
+{
+ /* assert: (fdt_magic(fdt) == FDT_SW_MAGIC) */
+ return fdt_last_comp_version(fdt);
+}
+
+/* 'complete' state: Enter this state after fdt_finish()
+ *
+ * Allowed functions: none
+ */
+
+static void *fdt_grab_space_(void *fdt, size_t len)
+{
+ int offset = fdt_size_dt_struct(fdt);
+ int spaceleft;
+
+ spaceleft = fdt_totalsize(fdt) - fdt_off_dt_struct(fdt)
+ - fdt_size_dt_strings(fdt);
+
+ if ((offset + len < offset) || (offset + len > spaceleft))
+ return NULL;
+
+ fdt_set_size_dt_struct(fdt, offset + len);
+ return fdt_offset_ptr_w_(fdt, offset);
+}
+
+int fdt_create_with_flags(void *buf, int bufsize, uint32_t flags)
+{
+ const size_t hdrsize = FDT_ALIGN(sizeof(struct fdt_header),
+ sizeof(struct fdt_reserve_entry));
+ void *fdt = buf;
+
+ if (bufsize < hdrsize)
+ return -FDT_ERR_NOSPACE;
+
+ if (flags & ~FDT_CREATE_FLAGS_ALL)
+ return -FDT_ERR_BADFLAGS;
+
+ memset(buf, 0, bufsize);
+
+ /*
+ * magic and last_comp_version keep intermediate state during the fdt
+ * creation process, which is replaced with the proper FDT format by
+ * fdt_finish().
+ *
+ * flags should be accessed with sw_flags().
+ */
+ fdt_set_magic(fdt, FDT_SW_MAGIC);
+ fdt_set_version(fdt, FDT_LAST_SUPPORTED_VERSION);
+ fdt_set_last_comp_version(fdt, flags);
+
+ fdt_set_totalsize(fdt, bufsize);
+
+ fdt_set_off_mem_rsvmap(fdt, hdrsize);
+ fdt_set_off_dt_struct(fdt, fdt_off_mem_rsvmap(fdt));
+ fdt_set_off_dt_strings(fdt, 0);
+
+ return 0;
+}
+
+int fdt_create(void *buf, int bufsize)
+{
+ return fdt_create_with_flags(buf, bufsize, 0);
+}
+
+int fdt_resize(void *fdt, void *buf, int bufsize)
+{
+ size_t headsize, tailsize;
+ char *oldtail, *newtail;
+
+ FDT_SW_PROBE(fdt);
+
+ headsize = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
+ tailsize = fdt_size_dt_strings(fdt);
+
+ if ((headsize + tailsize) > fdt_totalsize(fdt))
+ return -FDT_ERR_INTERNAL;
+
+ if ((headsize + tailsize) > bufsize)
+ return -FDT_ERR_NOSPACE;
+
+ oldtail = (char *)fdt + fdt_totalsize(fdt) - tailsize;
+ newtail = (char *)buf + bufsize - tailsize;
+
+ /* Two cases to avoid clobbering data if the old and new
+ * buffers partially overlap */
+ if (buf <= fdt) {
+ memmove(buf, fdt, headsize);
+ memmove(newtail, oldtail, tailsize);
+ } else {
+ memmove(newtail, oldtail, tailsize);
+ memmove(buf, fdt, headsize);
+ }
+
+ fdt_set_totalsize(buf, bufsize);
+ if (fdt_off_dt_strings(buf))
+ fdt_set_off_dt_strings(buf, bufsize);
+
+ return 0;
+}
+
+int fdt_add_reservemap_entry(void *fdt, uint64_t addr, uint64_t size)
+{
+ struct fdt_reserve_entry *re;
+ int offset;
+
+ FDT_SW_PROBE_MEMRSV(fdt);
+
+ offset = fdt_off_dt_struct(fdt);
+ if ((offset + sizeof(*re)) > fdt_totalsize(fdt))
+ return -FDT_ERR_NOSPACE;
+
+ re = (struct fdt_reserve_entry *)((char *)fdt + offset);
+ re->address = cpu_to_fdt64(addr);
+ re->size = cpu_to_fdt64(size);
+
+ fdt_set_off_dt_struct(fdt, offset + sizeof(*re));
+
+ return 0;
+}
+
+int fdt_finish_reservemap(void *fdt)
+{
+ int err = fdt_add_reservemap_entry(fdt, 0, 0);
+
+ if (err)
+ return err;
+
+ fdt_set_off_dt_strings(fdt, fdt_totalsize(fdt));
+ return 0;
+}
+
+int fdt_begin_node(void *fdt, const char *name)
+{
+ struct fdt_node_header *nh;
+ int namelen;
+
+ FDT_SW_PROBE_STRUCT(fdt);
+
+ namelen = strlen(name) + 1;
+ nh = fdt_grab_space_(fdt, sizeof(*nh) + FDT_TAGALIGN(namelen));
+ if (! nh)
+ return -FDT_ERR_NOSPACE;
+
+ nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE);
+ memcpy(nh->name, name, namelen);
+ return 0;
+}
+
+int fdt_end_node(void *fdt)
+{
+ fdt32_t *en;
+
+ FDT_SW_PROBE_STRUCT(fdt);
+
+ en = fdt_grab_space_(fdt, FDT_TAGSIZE);
+ if (! en)
+ return -FDT_ERR_NOSPACE;
+
+ *en = cpu_to_fdt32(FDT_END_NODE);
+ return 0;
+}
+
+static int fdt_add_string_(void *fdt, const char *s)
+{
+ char *strtab = (char *)fdt + fdt_totalsize(fdt);
+ int strtabsize = fdt_size_dt_strings(fdt);
+ int len = strlen(s) + 1;
+ int struct_top, offset;
+
+ offset = -strtabsize - len;
+ struct_top = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
+ if (fdt_totalsize(fdt) + offset < struct_top)
+ return 0; /* no more room :( */
+
+ memcpy(strtab + offset, s, len);
+ fdt_set_size_dt_strings(fdt, strtabsize + len);
+ return offset;
+}
+
+/* Must only be used to roll back in case of error */
+static void fdt_del_last_string_(void *fdt, const char *s)
+{
+ int strtabsize = fdt_size_dt_strings(fdt);
+ int len = strlen(s) + 1;
+
+ fdt_set_size_dt_strings(fdt, strtabsize - len);
+}
+
+static int fdt_find_add_string_(void *fdt, const char *s, int *allocated)
+{
+ char *strtab = (char *)fdt + fdt_totalsize(fdt);
+ int strtabsize = fdt_size_dt_strings(fdt);
+ const char *p;
+
+ *allocated = 0;
+
+ p = fdt_find_string_(strtab - strtabsize, strtabsize, s);
+ if (p)
+ return p - strtab;
+
+ *allocated = 1;
+
+ return fdt_add_string_(fdt, s);
+}
+
+int fdt_property_placeholder(void *fdt, const char *name, int len, void **valp)
+{
+ struct fdt_property *prop;
+ int nameoff;
+ int allocated;
+
+ FDT_SW_PROBE_STRUCT(fdt);
+
+ /* String de-duplication can be slow, _NO_NAME_DEDUP skips it */
+ if (sw_flags(fdt) & FDT_CREATE_FLAG_NO_NAME_DEDUP) {
+ allocated = 1;
+ nameoff = fdt_add_string_(fdt, name);
+ } else {
+ nameoff = fdt_find_add_string_(fdt, name, &allocated);
+ }
+ if (nameoff == 0)
+ return -FDT_ERR_NOSPACE;
+
+ prop = fdt_grab_space_(fdt, sizeof(*prop) + FDT_TAGALIGN(len));
+ if (! prop) {
+ if (allocated)
+ fdt_del_last_string_(fdt, name);
+ return -FDT_ERR_NOSPACE;
+ }
+
+ prop->tag = cpu_to_fdt32(FDT_PROP);
+ prop->nameoff = cpu_to_fdt32(nameoff);
+ prop->len = cpu_to_fdt32(len);
+ *valp = prop->data;
+ return 0;
+}
+
+int fdt_property(void *fdt, const char *name, const void *val, int len)
+{
+ void *ptr;
+ int ret;
+
+ ret = fdt_property_placeholder(fdt, name, len, &ptr);
+ if (ret)
+ return ret;
+ memcpy(ptr, val, len);
+ return 0;
+}
+
+int fdt_finish(void *fdt)
+{
+ char *p = (char *)fdt;
+ fdt32_t *end;
+ int oldstroffset, newstroffset;
+ uint32_t tag;
+ int offset, nextoffset;
+
+ FDT_SW_PROBE_STRUCT(fdt);
+
+ /* Add terminator */
+ end = fdt_grab_space_(fdt, sizeof(*end));
+ if (! end)
+ return -FDT_ERR_NOSPACE;
+ *end = cpu_to_fdt32(FDT_END);
+
+ /* Relocate the string table */
+ oldstroffset = fdt_totalsize(fdt) - fdt_size_dt_strings(fdt);
+ newstroffset = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
+ memmove(p + newstroffset, p + oldstroffset, fdt_size_dt_strings(fdt));
+ fdt_set_off_dt_strings(fdt, newstroffset);
+
+ /* Walk the structure, correcting string offsets */
+ offset = 0;
+ while ((tag = fdt_next_tag(fdt, offset, &nextoffset)) != FDT_END) {
+ if (tag == FDT_PROP) {
+ struct fdt_property *prop =
+ fdt_offset_ptr_w_(fdt, offset);
+ int nameoff;
+
+ nameoff = fdt32_to_cpu(prop->nameoff);
+ nameoff += fdt_size_dt_strings(fdt);
+ prop->nameoff = cpu_to_fdt32(nameoff);
+ }
+ offset = nextoffset;
+ }
+ if (nextoffset < 0)
+ return nextoffset;
+
+ /* Finally, adjust the header */
+ fdt_set_totalsize(fdt, newstroffset + fdt_size_dt_strings(fdt));
+
+ /* And fix up fields that were keeping intermediate state. */
+ fdt_set_last_comp_version(fdt, FDT_FIRST_SUPPORTED_VERSION);
+ fdt_set_magic(fdt, FDT_MAGIC);
+
+ return 0;
+}