ldelf, ta: add support for DT_INIT_ARRAY and DT_FINI_ARRAY

Adds support for running initialization and finalization functions in
TA ELF files. Such functions are used, for instance, by C++ compilers
to construct and destruct global objects. They can also be used in C
thanks to __attribute__((constructor)) and __attribute__((destructor)).

A global structure is added to libutee. ldelf is responsible for
filling it with the addresses of the functions pointer arrays present
in the ELF files whenever such a file is loaded. Since the number of
arrays is unknown at compile time (it depends on how many ELF files are
loaded, and whether they have constructors or destructors), memory is
allocated on the TA heap.

Two helper functions are introduced: __utee_call_elf_init_fn() and
__utee_call_elf_fini_fn(). They are used when the TA instance is
created and torn down, as well as by dlopen().

Signed-off-by: Jerome Forissier <jerome@forissier.org>
Tested-by: Jerome Forissier <jerome@forissier.org> (QEMU, QEMUv8, HiKey960 32/64)
Reviewed-by: Jens Wiklander <jens.wiklander@linaro.org>

6 files changed, 310 insertions, 5 deletions

diff --git a/ldelf/ta_elf.c b/ldelf/ta_elf.c
index 733592e4..5c2a2566 100644
--- a/ldelf/ta_elf.c
+++ b/ldelf/ta_elf.c
@@ -913,9 +913,14 @@ void ta_elf_load_main(const TEE_UUID *uuid, uint32_t *is_32bit, uint64_t *sp,
 void ta_elf_finalize_load_main(uint64_t *entry)
 {
 	struct ta_elf *elf = TAILQ_FIRST(&main_elf_queue);
+	TEE_Result res = TEE_SUCCESS;
 
 	assert(elf->is_main);
 
+	res = ta_elf_set_init_fini_info(elf->is_32bit);
+	if (res)
+		err(res, "ta_elf_set_init_fini_info");
+
 	if (elf->is_legacy)
 		*entry = elf->head->depr_entry;
 	else
@@ -1193,5 +1198,213 @@ TEE_Result ta_elf_add_library(const TEE_UUID *uuid)
 		DMSG("ELF (%pUl) at %#"PRIxVA,
 		     (void *)&elf->uuid, elf->load_addr);
 
+	return ta_elf_set_init_fini_info(ta->is_32bit);
+}
+
+/* Get address/size of .init_array and .fini_array from the dynamic segment */
+static void get_init_fini_array(struct ta_elf *elf, unsigned int type,
+				vaddr_t addr, size_t memsz, vaddr_t *init,
+				size_t *init_cnt, vaddr_t *fini,
+				size_t *fini_cnt)
+{
+	size_t addrsz = 0;
+	size_t dyn_entsize = 0;
+	size_t num_dyns = 0;
+	size_t n = 0;
+	unsigned int tag = 0;
+	size_t val = 0;
+
+	assert(type == PT_DYNAMIC);
+
+	if (elf->is_32bit) {
+		dyn_entsize = sizeof(Elf32_Dyn);
+		addrsz = 4;
+	} else {
+		dyn_entsize = sizeof(Elf64_Dyn);
+		addrsz = 8;
+	}
+
+	assert(!(memsz % dyn_entsize));
+	num_dyns = memsz / dyn_entsize;
+
+	for (n = 0; n < num_dyns; n++) {
+		read_dyn(elf, addr, n, &tag, &val);
+		if (tag == DT_INIT_ARRAY)
+			*init = val + elf->load_addr;
+		else if (tag == DT_FINI_ARRAY)
+			*fini = val + elf->load_addr;
+		else if (tag == DT_INIT_ARRAYSZ)
+			*init_cnt = val / addrsz;
+		else if (tag == DT_FINI_ARRAYSZ)
+			*fini_cnt = val / addrsz;
+	}
+}
+
+/* Get address/size of .init_array and .fini_array in @elf (if present) */
+static void elf_get_init_fini_array(struct ta_elf *elf, vaddr_t *init,
+				    size_t *init_cnt, vaddr_t *fini,
+				    size_t *fini_cnt)
+{
+	size_t n = 0;
+
+	if (elf->is_32bit) {
+		Elf32_Phdr *phdr = elf->phdr;
+
+		for (n = 0; n < elf->e_phnum; n++) {
+			if (phdr[n].p_type == PT_DYNAMIC) {
+				get_init_fini_array(elf, phdr[n].p_type,
+						    phdr[n].p_vaddr,
+						    phdr[n].p_memsz,
+						    init, init_cnt, fini,
+						    fini_cnt);
+				return;
+			}
+		}
+	} else {
+		Elf64_Phdr *phdr = elf->phdr;
+
+		for (n = 0; n < elf->e_phnum; n++) {
+			if (phdr[n].p_type == PT_DYNAMIC) {
+				get_init_fini_array(elf, phdr[n].p_type,
+						    phdr[n].p_vaddr,
+						    phdr[n].p_memsz,
+						    init, init_cnt, fini,
+						    fini_cnt);
+				return;
+			}
+		}
+	}
+}
+
+static TEE_Result realloc_ifs(vaddr_t va, size_t cnt, bool is_32bit)
+{
+	struct __init_fini_info32 *info32 = (struct __init_fini_info32 *)va;
+	struct __init_fini_info *info = (struct __init_fini_info *)va;
+	struct __init_fini32 *ifs32 = NULL;
+	struct __init_fini *ifs = NULL;
+	size_t prev_cnt = 0;
+	void *ptr = NULL;
+
+	if (is_32bit) {
+		ptr = (void *)(vaddr_t)info32->ifs;
+		ptr = realloc(ptr, cnt * sizeof(struct __init_fini32));
+		if (!ptr)
+			return TEE_ERROR_OUT_OF_MEMORY;
+		ifs32 = ptr;
+		prev_cnt = info32->size;
+		if (cnt > prev_cnt)
+			memset(ifs32 + prev_cnt, 0,
+			       (cnt - prev_cnt) * sizeof(*ifs32));
+		info32->ifs = (uint32_t)(vaddr_t)ifs32;
+		info32->size = cnt;
+	} else {
+		ptr = realloc(info->ifs, cnt * sizeof(struct __init_fini));
+		if (!ptr)
+			return TEE_ERROR_OUT_OF_MEMORY;
+		ifs = ptr;
+		prev_cnt = info->size;
+		if (cnt > prev_cnt)
+			memset(ifs + prev_cnt, 0,
+			       (cnt - prev_cnt) * sizeof(*ifs));
+		info->ifs = ifs;
+		info->size = cnt;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static void fill_ifs(vaddr_t va, size_t idx, struct ta_elf *elf, bool is_32bit)
+{
+	struct __init_fini_info32 *info32 = (struct __init_fini_info32 *)va;
+	struct __init_fini_info *info = (struct __init_fini_info *)va;
+	struct __init_fini32 *ifs32 = NULL;
+	struct __init_fini *ifs = NULL;
+	size_t init_cnt = 0;
+	size_t fini_cnt = 0;
+	vaddr_t init = 0;
+	vaddr_t fini = 0;
+
+	if (is_32bit) {
+		assert(idx < info32->size);
+		ifs32 = &((struct __init_fini32 *)(vaddr_t)info32->ifs)[idx];
+
+		if (ifs32->flags & __IFS_VALID)
+			return;
+
+		elf_get_init_fini_array(elf, &init, &init_cnt, &fini,
+					&fini_cnt);
+
+		ifs32->init = (uint32_t)init;
+		ifs32->init_size = init_cnt;
+
+		ifs32->fini = (uint32_t)fini;
+		ifs32->fini_size = fini_cnt;
+
+		ifs32->flags |= __IFS_VALID;
+	} else {
+		assert(idx < info->size);
+		ifs = &info->ifs[idx];
+
+		if (ifs->flags & __IFS_VALID)
+			return;
+
+		elf_get_init_fini_array(elf, &init, &init_cnt, &fini,
+					&fini_cnt);
+
+		ifs->init = (void (**)(void))init;
+		ifs->init_size = init_cnt;
+
+		ifs->fini = (void (**)(void))fini;
+		ifs->fini_size = fini_cnt;
+
+		ifs->flags |= __IFS_VALID;
+	}
+}
+
+/*
+ * Set or update __init_fini_info in the TA with information from the ELF
+ * queue
+ */
+TEE_Result ta_elf_set_init_fini_info(bool is_32bit)
+{
+	struct __init_fini_info *info = NULL;
+	TEE_Result res = TEE_SUCCESS;
+	struct ta_elf *elf = NULL;
+	vaddr_t info_va = 0;
+	size_t cnt = 0;
+
+	res = ta_elf_resolve_sym("__init_fini_info", &info_va, NULL);
+	if (res) {
+		if (res == TEE_ERROR_ITEM_NOT_FOUND) {
+			/* Older TA */
+			return TEE_SUCCESS;
+		}
+		return res;
+	}
+	assert(info_va);
+
+	info = (struct __init_fini_info *)info_va;
+	if (info->reserved)
+		return TEE_ERROR_NOT_SUPPORTED;
+
+	TAILQ_FOREACH(elf, &main_elf_queue, link)
+		cnt++;
+
+	/* Queue has at least one file (main) */
+	assert(cnt);
+
+	res = realloc_ifs(info_va, cnt, is_32bit);
+	if (res)
+		goto err;
+
+	cnt = 0;
+	TAILQ_FOREACH(elf, &main_elf_queue, link) {
+		fill_ifs(info_va, cnt, elf, is_32bit);
+		cnt++;
+	}
+
 	return TEE_SUCCESS;
+err:
+	free(info);
+	return res;
 }
diff --git a/ldelf/ta_elf.h b/ldelf/ta_elf.h
index 373a2da0..7e4ad644 100644
--- a/ldelf/ta_elf.h
+++ b/ldelf/ta_elf.h
@@ -104,5 +104,6 @@ static inline void ta_elf_stack_trace_a64(uint64_t fp __unused,
 TEE_Result ta_elf_resolve_sym(const char *name, vaddr_t *val,
 			      struct ta_elf *elf);
 TEE_Result ta_elf_add_library(const TEE_UUID *uuid);
+TEE_Result ta_elf_set_init_fini_info(bool is_32bit);
 
 #endif /*TA_ELF_H*/
diff --git a/lib/libdl/dlfcn.c b/lib/libdl/dlfcn.c
index b2b076e8..d1d7e31a 100644
--- a/lib/libdl/dlfcn.c
+++ b/lib/libdl/dlfcn.c
@@ -10,6 +10,7 @@
 #include <string.h>
 #include <tee_api.h>
 #include <tee_internal_api_extensions.h>
+#include <user_ta_header.h>
 
 static TEE_TASessionHandle sess = TEE_HANDLE_NULL;
 static size_t hcount;
@@ -63,6 +64,8 @@ void *dlopen(const char *filename, int flags)
 		res = invoke_system_pta(PTA_SYSTEM_DLOPEN, param_types, params);
 		if (res)
 			goto err;
+
+		__utee_call_elf_init_fn();
 	}
 
 	hcount++;
diff --git a/lib/libutee/arch/arm/user_ta_entry.c b/lib/libutee/arch/arm/user_ta_entry.c
index 41ff4d8e..b317bb30 100644
--- a/lib/libutee/arch/arm/user_ta_entry.c
+++ b/lib/libutee/arch/arm/user_ta_entry.c
@@ -33,6 +33,42 @@ extern struct ta_head ta_head;
 
 uint32_t ta_param_types;
 TEE_Param ta_params[TEE_NUM_PARAMS];
+struct __init_fini_info __init_fini_info;
+
+void __utee_call_elf_init_fn(void)
+{
+	size_t n = __init_fini_info.size;
+	size_t i = 0;
+	size_t j = 0;
+
+	for (i = 1; i <= n; i++) {
+		/* Reverse order: dependencies first */
+		struct __init_fini *ifs = &__init_fini_info.ifs[n - i];
+
+		if (!(ifs->flags & __IFS_INIT_HAS_RUN))
+			for (j = 0; j < ifs->init_size; j++)
+				ifs->init[j]();
+
+		ifs->flags |= __IFS_INIT_HAS_RUN;
+	}
+}
+
+void __utee_call_elf_fini_fn(void)
+{
+	size_t n = __init_fini_info.size;
+	size_t i = 0;
+	size_t j = 0;
+
+	for (i = 0; i < n; i++) {
+		struct __init_fini *ifs = &__init_fini_info.ifs[i];
+
+		if (!(ifs->flags & __IFS_FINI_HAS_RUN))
+			for (j = 1; j <= ifs->fini_size; j++)
+				ifs->fini[ifs->fini_size - j]();
+
+		ifs->flags |= __IFS_FINI_HAS_RUN;
+	}
+}
 
 static TEE_Result init_instance(void)
 {
@@ -40,6 +76,7 @@ static TEE_Result init_instance(void)
 	__utee_gprof_init();
 	malloc_add_pool(ta_heap, ta_heap_size);
 	_TEE_MathAPI_Init();
+	__utee_call_elf_init_fn();
 	return TA_CreateEntryPoint();
 }
 
@@ -47,6 +84,7 @@ static void uninit_instance(void)
 {
 	__utee_gprof_fini();
 	TA_DestroyEntryPoint();
+	__utee_call_elf_fini_fn();
 }
 
 static void ta_header_save_params(uint32_t param_types,
diff --git a/lib/libutee/include/user_ta_header.h b/lib/libutee/include/user_ta_header.h
index f9e7c071..6f81fd64 100644
--- a/lib/libutee/include/user_ta_header.h
+++ b/lib/libutee/include/user_ta_header.h
@@ -73,6 +73,53 @@ unsigned long ftrace_return(void);
 void __ftrace_return(void);
 #endif
 
+/*
+ * Pointers to ELF initialization and finalization functions are extracted by
+ * ldelf and stored on the TA heap. They can be accessed via the TA global
+ * variable __init_fini_info::ifs, but the functions are meant to called via
+ * __utee_call_elf_init_fn() and __utee_call_elf_fini_fn().
+ */
+
+struct __init_fini {
+	uint32_t flags;
+	uint16_t init_size;
+	uint16_t fini_size;
+
+	void (**init)(void); /* @init_size entries */
+	void (**fini)(void); /* @fini_size entries */
+};
+
+#define __IFS_VALID		BIT(0)
+#define __IFS_INIT_HAS_RUN	BIT(1)
+#define __IFS_FINI_HAS_RUN	BIT(2)
+
+struct __init_fini_info {
+	uint32_t reserved;
+	uint16_t size;
+	uint16_t pad;
+	struct __init_fini *ifs; /* @size entries */
+};
+
+/* 32-bit variants for a 64-bit ldelf to access a 32-bit TA */
+
+struct __init_fini32 {
+	uint32_t flags;
+	uint16_t init_size;
+	uint16_t fini_size;
+	uint32_t init;
+	uint32_t fini;
+};
+
+struct __init_fini_info32 {
+	uint32_t reserved;
+	uint16_t size;
+	uint16_t pad;
+	uint32_t ifs;
+};
+
+void __utee_call_elf_init_fn(void);
+void __utee_call_elf_fini_fn(void);
+
 #define TA_PROP_STR_SINGLE_INSTANCE	"gpd.ta.singleInstance"
 #define TA_PROP_STR_MULTI_SESSION	"gpd.ta.multiSession"
 #define TA_PROP_STR_KEEP_ALIVE		"gpd.ta.instanceKeepAlive"
diff --git a/ta/arch/arm/link.mk b/ta/arch/arm/link.mk
index c33f6d15..9c98fe1c 100644
--- a/ta/arch/arm/link.mk
+++ b/ta/arch/arm/link.mk
@@ -34,15 +34,18 @@ link-ldflags += -z max-page-size=4096 # OP-TEE always uses 4K alignment
 link-ldflags += --as-needed # Do not add dependency on unused shlib
 link-ldflags += $(link-ldflags$(sm))
 
-ifeq ($(CFG_FTRACE_SUPPORT),y)
 $(link-out-dir$(sm))/dyn_list:
 	@$(cmd-echo-silent) '  GEN     $@'
 	$(q)mkdir -p $(dir $@)
-	$(q)echo "{__ftrace_info;};" >$@
+	$(q)echo "{" >$@
+	$(q)echo "__init_fini_info;" >>$@
+ifeq ($(CFG_FTRACE_SUPPORT),y)
+	$(q)echo "__ftrace_info;" >>$@
+endif
+	$(q)echo "};" >>$@
 link-ldflags += --dynamic-list $(link-out-dir$(sm))/dyn_list
-ftracedep = $(link-out-dir$(sm))/dyn_list
+dynlistdep = $(link-out-dir$(sm))/dyn_list
 cleanfiles += $(link-out-dir$(sm))/dyn_list
-endif
 
 link-ldadd  = $(user-ta-ldadd) $(addprefix -L,$(libdirs))
 link-ldadd += --start-group $(addprefix -l,$(libnames)) --end-group
@@ -68,7 +71,7 @@ $(link-script-pp$(sm)): $(link-script$(sm)) $(conf-file) $(link-script-pp-makefi
 
 $(link-out-dir$(sm))/$(user-ta-uuid).elf: $(objs) $(libdeps) \
 					  $(link-script-pp$(sm)) \
-					  $(ftracedep) \
+					  $(dynlistdep) \
 					  $(additional-link-deps)
 	@$(cmd-echo-silent) '  LD      $$@'
 	$(q)$(LD$(sm)) $(ldargs-$(user-ta-uuid).elf) -o $$@