[Feature]add MT2731_MP2_MR2_SVN388 baseline version
Change-Id: Ief04314834b31e27effab435d3ca8ba33b499059
diff --git a/src/bsp/lk/app/fitboot/fitboot.c b/src/bsp/lk/app/fitboot/fitboot.c
new file mode 100644
index 0000000..dd2c288
--- /dev/null
+++ b/src/bsp/lk/app/fitboot/fitboot.c
@@ -0,0 +1,706 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include <app.h>
+#include <assert.h>
+#include <boot_mode.h>
+#include <errno.h>
+#include <libfdt.h>
+#include <kernel/event.h>
+#include <kernel/thread.h>
+#include <kernel/vm.h>
+#include <lib/mempool.h>
+#ifdef AB_UPGRADE_APP
+#include <lib/bio.h>
+#endif
+#include <platform.h>
+#include <platform/mtk_key.h>
+#include <platform/mtk_wdt.h>
+#include <trace.h>
+
+#include "fit.h"
+#ifdef RTC_CHECK_FASTBOOT
+#include "rtc.h"
+#endif
+
+#ifdef SET_FDT_EMI_INFO
+#include "platform/emi_info_v1.h"
+#endif
+
+#define LOCAL_TRACE 0
+
+/* BL33 load and entry point address */
+#define CFG_BL33_LOAD_EP_ADDR (BL33_ADDR)
+#define ERR_ADDR (0xffffffff)
+
+#ifdef AB_UPGRADE_APP
+#define UPG_SUCCEED 2
+#endif
+
+typedef void (*jump32_func_type)(uint32_t addr, uint32_t arg1, uint32_t arg2) __NO_RETURN;
+typedef void (*jump64_func_type)(uint64_t bl31_addr, uint64_t bl33_addr, uint64_t arg1) __NO_RETURN;
+
+struct fit_load_data {
+ char *part_name;
+ char *recovery_part_name;
+ void *buf;
+ u32 boot_mode;
+ ulong kernel_entry;
+ ulong dtb_load;
+ ulong trustedos_entry;
+};
+
+#ifdef AB_UPGRADE_APP
+typedef struct boot_flag boot_flag;
+struct boot_flag {
+ int lastboot;
+ int usea;
+ int useb;
+ int current;
+};
+#endif
+
+/* global variables, also used in dl_commands.c */
+void *kernel_buf;
+void *tz_buf;
+void *bl33_buf;
+
+__WEAK bool plat_fixup_hook(void* bootimg_dtb_load, ...);
+
+#if ARCH_ARM64
+void mtk_sip(uint32_t smc_fid, uint64_t bl31_addr, uint64_t bl33_addr)
+{
+ jump64_func_type jump64_func = (jump64_func_type)bl31_addr;
+ (*jump64_func)(bl31_addr, bl33_addr, 0UL);
+}
+#endif
+
+void prepare_bl2_exit(ulong smc_fid, ulong bl31_addr, ulong bl33_addr, ulong arg1)
+{
+#if ARCH_ARM64
+ /* switch to el3 via smc, and will jump to mtk_sip from smc handler */
+ __asm__ volatile("smc #0\n\t");
+#else
+ jump32_func_type jump32_func = (jump32_func_type)bl31_addr;
+ (*jump32_func)(bl33_addr, 0, 0);
+#endif
+}
+
+#pragma GCC push_options
+#pragma GCC optimize("O1")
+static void setup_bl33(uint *bl33, ulong fdt, ulong kernel_ep)
+{
+ bl33[12] = (ulong)fdt;
+ bl33[14] = (unsigned)0;
+ bl33[16] = (unsigned)0;
+ bl33[18] = (unsigned)0;
+ bl33[20] = (ulong)kernel_ep;
+ bl33[21] = (ulong)0;
+ bl33[22] = (unsigned)MACH_TYPE;
+}
+#pragma GCC pop_options
+
+static int extract_fdt(void *fdt, int size)
+{
+ int ret = 0;
+
+ /* DTB maximum size is 2MB */
+ ret = fdt_open_into(fdt, fdt, size);
+ if (ret) {
+ dprintf(CRITICAL, "open fdt failed\n");
+ return ret;
+ }
+ ret = fdt_check_header(fdt);
+ if (ret) {
+ dprintf(CRITICAL, "check fdt failed\n");
+ return ret;
+ }
+
+ return ret;
+}
+
+static bool check_uart_enter(void)
+{
+ char c;
+
+ if (platform_dgetc(&c, false) != 0)
+ return false;
+ return (c == 13);
+}
+
+#ifdef AB_UPGRADE_APP
+int set_currently_boot_flag(int last_flag, int current_flag, const char *part_name)
+{
+ int ret;
+ long len = 0;
+ u32 writesize = 2048;
+ int index = -1;
+ unsigned long long ptn = 0;
+ unsigned long long size = 0;
+ char *buf;
+ boot_flag set_flag;
+
+ /* read partition */
+ struct bdev *nand_MISC = bio_open_by_label("misc");
+ if (!nand_MISC) {
+ LTRACEF("open misc partition failed.\n");
+ return 1;
+ }
+ buf = malloc(writesize);
+ if (buf == NULL) {
+ LTRACEF("malloc for writesize failed.\n");
+ return 1;
+ }
+ memset(buf, 0, writesize);
+
+ len = bio_read(nand_MISC, buf, 0, sizeof(boot_flag));
+ if (len < 0) {
+ dprintf(CRITICAL, "%s read error. LINE: %d\n", part_name, __LINE__);
+ free(buf);
+ buf = NULL;
+ return -1;
+ }
+ /* dump flag for debug */
+ dprintf(CRITICAL, "current boot flag is %d\n", current_flag);
+ /* set currently flag to buf */
+ set_flag.lastboot = last_flag;
+ set_flag.current = current_flag;
+ set_flag.usea = (int)-1;
+ dprintf(CRITICAL, "last_flag boot flag is %d\n", last_flag);
+ set_flag.useb = (int)-1;
+ memset(buf, 0, writesize);
+ memcpy(buf, (void *)&set_flag, sizeof(boot_flag));
+ /* write buf to offset 0, which size is 2048 */
+ len = bio_write(nand_MISC, (char *)buf, 0, (u32)writesize);
+ if (len <= 0) {
+ dprintf(CRITICAL, "nand write fail, return : %d, error: %s\n",len, strerror(errno));
+ dprintf(CRITICAL, "buf: %s\n", buf);
+ ret = -1;
+ }
+ else {
+ dprintf(CRITICAL, "set flag: lastboot = %d, use A = %d, use B = %d, current = %d\n", set_flag.lastboot, set_flag.usea, set_flag.useb, set_flag.current);
+ ret = 0;
+ }
+
+ if (buf) {
+ free(buf);
+ buf = NULL;
+ }
+ return ret;
+}
+
+int check_boot_partition(const char *part_name)
+{
+ int ret = 0;
+ boot_flag flag;
+ int boot = 0;
+
+ struct bdev *nand_MISC = bio_open_by_label("misc");
+ int len = -1;
+ char *buf;
+
+ if (!nand_MISC) {
+ printf("failed to open MISC\n");
+ return 0;
+ }
+ printf("open MISC successfully\n");
+
+ /* read partition */
+ buf = malloc(sizeof(boot_flag));
+ if (buf == NULL) {
+ LTRACEF("malloc for boot_flag failed.\n");
+ return 2;
+ }
+
+ len = bio_read(nand_MISC, buf, 0, sizeof(boot_flag));
+ if (len < 0) {
+ dprintf(CRITICAL, "read %s: boot flag read error. LINE: %d\n", part_name, __LINE__);
+ free(buf);
+ buf = NULL;
+ return -1;
+ }
+ memcpy(&flag, (void *)buf, sizeof(boot_flag));
+
+ /* dump flag for debug */
+ dprintf(CRITICAL, "lastboot = %d, use A = %d, use B = %d, current = %d\n", flag.lastboot, flag.usea, flag.useb, flag.current);
+
+ /* make dicision */
+ if (flag.lastboot == 0) {
+ if (flag.useb == UPG_SUCCEED) {
+ boot = 1;
+ dprintf(CRITICAL,"***last succeed boot from A system,upgrade B succeed***\n");
+ dprintf(CRITICAL,"***now boot from system B***\n");
+ } else {
+ boot = 0;
+ dprintf(CRITICAL,"***last succeed boot from A system,upgrade B failed or no upgrade B***\n");
+ dprintf(CRITICAL,"***now boot from system A***\n");
+ }
+ } else if (flag.lastboot == 1) {
+ if (flag.usea == UPG_SUCCEED) {
+ boot = 0;
+ dprintf(CRITICAL,"***last succeed boot from B system,upgrade A succeed***\n");
+ dprintf(CRITICAL,"***now boot from system A***\n");
+ } else {
+ boot = 1;
+ dprintf(CRITICAL,"***last succeed boot from B system,upgrade A failed or no upgrade A***\n");
+ dprintf(CRITICAL,"***now boot from system B***\n");
+ }
+ } else {
+ dprintf(CRITICAL, "boot flag is not match, use default boot partition\n");
+ boot = 0;
+ }
+
+ if ((flag.current != boot) || (flag.usea == UPG_SUCCEED) || (flag.useb == UPG_SUCCEED)) {
+ ret = bio_erase(nand_MISC, 0, nand_MISC->total_size); //erase total_size
+ printf("bio erase ret %d\n", ret);
+ ret = set_currently_boot_flag(flag.lastboot, boot, part_name);
+ if (ret!=0)
+ dprintf(CRITICAL, "set flags fail. LINE: %d\n", __LINE__);
+ }
+ if (buf) {
+ free(buf);
+ buf = NULL;
+ }
+ return boot;
+}
+
+static int cmdlineoverlay(void *boot_dtb, char *cmdline, int len)
+{
+ int chosen_node_offset = 0;
+ int ret = -1;
+ char separator[2] = {0};
+ char str_nand[32] = "ubi.mtd=";
+ char str_emmc[32] = "root=/dev/mmcblk0p";
+ char str_real[32] = {0};
+ int header_length = 0;
+ ret = extract_fdt(boot_dtb, MAX_DTB_SIZE);
+ if (ret != 0) {
+ dprintf(CRITICAL, "extract_fdt error.\n");
+ return -1;
+ }
+
+ chosen_node_offset = fdt_path_offset(boot_dtb, "/chosen");
+ char *cmdline_read;
+ int length;
+ cmdline_read = fdt_getprop(boot_dtb, chosen_node_offset, "bootargs", &length);
+ dprintf(CRITICAL, "dtsi cmdline: %s ,lenth:%zu\n", cmdline_read, strlen(cmdline_read));
+ char *pos1;
+ char *pos2;
+
+ if ((pos1 = strstr(cmdline_read,str_nand)))
+ {
+ separator[0] = ',';
+ header_length = strlen(str_nand);
+ strncpy(str_real, str_nand, header_length);
+ }
+ else if ((pos1 = strstr(cmdline_read,str_emmc)))
+ {
+ separator[0] = ' ';
+ header_length = strlen(str_emmc);
+ strncpy(str_real, str_emmc, header_length);
+ }
+ else
+ {
+ dprintf(CRITICAL, "no ubi.mtd= or root=/dev/mmcblk0p in cmdline, error!\n");
+ return -1;
+ }
+
+ pos2 = strstr(pos1, separator);
+ if (pos2 == NULL) {
+ dprintf(CRITICAL, "can not find separator in cmdline, error!\n");
+ return -1;
+ }
+ if ((pos2 - pos1 - header_length) <= 0) {
+ dprintf(CRITICAL, "no part number in cmdline, error!\n");
+ return -1;
+ }
+
+ char mtdnum_str[3];
+ char mtdnum_str_new[3];
+ strncpy(mtdnum_str, pos1 + header_length, (pos2 - pos1 - header_length));
+ mtdnum_str[pos2 - pos1 - header_length] = '\0';
+ int mtdnum = atoi(mtdnum_str);
+ mtdnum ++;
+ sprintf(mtdnum_str_new, "%d", mtdnum);
+ if (mtdnum >= 10) {
+ char half_before[1024] = {'\0'};
+ char half_behind[1024] = {'\0'};
+ strncpy(half_before, cmdline_read, pos2 - cmdline_read);
+ char *pos3 = strstr(half_before, str_real);
+ strncpy(pos3 + header_length, mtdnum_str_new, 2);
+ strncpy(half_behind, pos2, strlen(pos2) + 1);
+ cmdline_read = strncat(half_before, half_behind, strlen(half_behind));
+
+ } else {
+ strncpy(pos1 + header_length, mtdnum_str_new, 1);
+ }
+ printf("cmdline new: %s , length: %zu", cmdline_read, strlen(cmdline_read));
+ ret = fdt_setprop(boot_dtb, chosen_node_offset, "bootargs", cmdline_read, strlen(cmdline_read) + 1);
+ if (ret != 0) {
+ dprintf(CRITICAL, "fdt_setprop error.\n");
+ return -1;
+ }
+ ret = fdt_pack(boot_dtb);
+ if (ret != 0) {
+ dprintf(CRITICAL, "fdt_pack error.\n");
+ return -1;
+ }
+
+ return 0;
+}
+#endif
+
+static bool download_check(void)
+{
+ if (check_fastboot_mode()) {
+ set_clr_fastboot_mode(false);
+ dprintf(CRITICAL, "download_check: check_fastboot_mode\n");
+ return true;
+#ifdef RTC_CHECK_FASTBOOT
+ } else if (rtc_bootloader_check()) {
+ rtc_bootloader_set_clr(false);
+ printf("download_check done");
+ return true;
+#endif
+ } else {
+ dprintf(CRITICAL, "download_check: check_uart_enter:%d, check_download_key:%d\n",
+ check_uart_enter(), check_download_key());
+ return (check_uart_enter() || check_download_key());
+ }
+}
+
+static bool recovery_check(void)
+{
+ if (check_recovery_mode()) {
+ set_clr_recovery_mode(false);
+ return true;
+ } else
+ return false;
+}
+
+static int fit_load_images(void *fit, struct fit_load_data *fit_data)
+{
+ int ret;
+
+ /* TODO: decide verify policy with config. */
+ dprintf(CRITICAL, "verify fit conf sig: %s\n", fit_data->part_name);
+ ret = fit_conf_verify_sig(NULL, fit);
+ if (ret < 0)
+ return ret;
+
+ ret = fit_load_image(NULL, "kernel", fit, NULL, NULL,
+ (paddr_t *)&fit_data->kernel_entry, true);
+ if (ret && (ret != -ENOENT)) {
+ dprintf(CRITICAL, "%s load kernel failed\n", fit_data->part_name);
+ return ret;
+ }
+
+ ret = fit_load_image(NULL, "tee", fit, NULL, NULL,
+ (paddr_t *)&fit_data->trustedos_entry, true);
+ if (ret && (ret != -ENOENT)) {
+ dprintf(CRITICAL, "%s load trustedos failed\n", fit_data->part_name);
+ return ret;
+ }
+
+ ret = fit_load_image(NULL, "ramdisk", fit, NULL, NULL, NULL, true);
+ if (ret && (ret != -ENOENT)) {
+ dprintf(CRITICAL, "%s load ramdisk failed\n", fit_data->part_name);
+ return ret;
+ }
+
+ ret = fit_load_image(NULL, "fdt", fit, (addr_t *)&fit_data->dtb_load, NULL,
+ NULL, true);
+ if (ret && (ret != -ENOENT)) {
+ fit_data->dtb_load = ERR_ADDR;
+ dprintf(CRITICAL, "%s load fdt failed\n", fit_data->part_name);
+ return ret;
+ }
+
+ return 0;
+}
+
+
+static int fit_load_thread(void *arg)
+{
+ int err;
+ struct fit_load_data *fit_data = (struct fit_load_data *)arg;
+
+ if (fit_data->boot_mode == FASTBOOT_BOOT) {
+ err = fit_load_images(fit_data->buf, fit_data);
+ return err;
+ }
+
+ while (fit_data->boot_mode == NORMAL_BOOT) {
+ err = fit_get_image(fit_data->part_name, &fit_data->buf);
+ if (err)
+ break;
+
+ err = fit_load_images(fit_data->buf, fit_data);
+ if (err)
+ break;
+
+ return 0;
+ }
+
+#ifdef AB_UPGRADE_APP
+ /* For ab upgrade system, there is no recovery mode */
+#else
+ dprintf(CRITICAL, "%s try recovery mode !!\n", fit_data->part_name);
+ // RECOVERY_BOOT
+ err = fit_get_image(fit_data->recovery_part_name, &fit_data->buf);
+ if (err)
+ return err;
+
+ err = fit_load_images(fit_data->buf, fit_data);
+#endif
+
+ return err;
+}
+
+extern void ext_boot(void);
+static void fitboot_task(const struct app_descriptor *app, void *args)
+{
+ void *fit, *dtbo_buf;
+ struct fit_load_data tz, bootimg;
+ thread_t *tz_t, *bootimg_t;
+ int ret_tz, ret_bootimg;
+
+ int ret;
+ u32 boot_mode = NORMAL_BOOT;
+
+ uint bl33[] = { 0xea000005, /* b BL33_32_ENTRY | ands x5, x0, x0 */
+ 0x58000160, /* .word 0x58000160 | ldr x0, _X0 */
+ 0x58000181, /* .word 0x58000181 | ldr x1, _X1 */
+ 0x580001a2, /* .word 0x580001a2 | ldr x2, _X2 */
+ 0x580001c3, /* .word 0x580001c3 | ldr x3, _X3 */
+ 0x580001e4, /* .word 0x580001e4 | ldr x4, _X4 */
+ 0xd61f0080, /* .word 0xd61f0080 | br x4 */
+ /* BL33_32_ENTRY: | */
+ 0xe59f0030, /* ldr r0, _R0 | .word 0xe59f0030 */
+ 0xe59f1030, /* ldr r1, _R1 | .word 0xe59f1030 */
+ 0xe59f2004, /* ldr r2, _X0 | .word 0xe59f2004 */
+ 0xe59ff020, /* ldr pc, _X4 | .word 0xe59ff020 */
+ 0x00000000, /* .word 0x00000000 */
+ 0x00000000, /* _X0: .word 0x00000000 */
+ 0x00000000, /* .word 0x00000000 */
+ 0x00000000, /* _X1: .word 0x00000000 */
+ 0x00000000, /* .word 0x00000000 */
+ 0x00000000, /* _X2: .word 0x00000000 */
+ 0x00000000, /* .word 0x00000000 */
+ 0x00000000, /* _X3: .word 0x00000000 */
+ 0x00000000, /* .word 0x00000000 */
+ 0x00000000, /* _X4: .word 0x00000000 */
+ 0x00000000, /* _R0: .word 0x00000000 */
+ 0x00000000, /* _R1: .word 0x00000000 */
+ 0x00000000 /* .word 0x00000000 */
+ };
+
+ /* alloc kernel and tz buffer from mempool */
+ kernel_buf = mempool_alloc(MAX_KERNEL_SIZE, MEMPOOL_ANY);
+ tz_buf = mempool_alloc(MAX_TEE_DRAM_SIZE, MEMPOOL_ANY);
+ if (!kernel_buf || !tz_buf) {
+ dprintf(CRITICAL, "alloc buf fail, kernel %p, tz %p\n",
+ kernel_buf, tz_buf);
+ return;
+ }
+#ifdef AB_UPGRADE_APP
+ /* For ab upgrade system, there is no recovery mode */
+#else
+ /* recovery */
+ if (recovery_check()) {
+ boot_mode = RECOVERY_BOOT;
+ }
+#endif
+
+ /* fastboot */
+ if (download_check()) {
+FASTBOOT:
+ ext_boot();
+ boot_mode = FASTBOOT_BOOT;
+ }
+
+ bootimg.part_name = (char *)BOOT_PART_NAME;
+ tz.part_name = (char *)TZ_PART_NAME;
+
+#ifdef AB_UPGRADE_APP
+ /* disable wdt */
+ /*1.choose A/B boot & tz img.*/
+ int boot_part = 0;
+ boot_part = check_boot_partition("misc");
+ if (boot_part == 0) {
+ dprintf(CRITICAL, "choose first boot partition:%s , tee choose: %s\n",(char *)BOOT_PART_NAME, (char *)TZ_PART_NAME);
+ bootimg.part_name = (char *)BOOT_PART_NAME;
+ tz.part_name = (char *)TZ_PART_NAME;
+ //cmdlineoverlay(bootimg.dtb_load, NULL, 0); from b partition,need to set
+
+ } else if (boot_part == 1) {
+ dprintf(CRITICAL, "choose second boot partition: %s , tee choose: %s\n", (char *)RECOVERY_BOOT_PART_NAME, (char *)RECOVERY_TZ_PART_NAME);
+ bootimg.part_name = (char *)RECOVERY_BOOT_PART_NAME;
+ tz.part_name = (char *)RECOVERY_TZ_PART_NAME;
+
+ } else {
+ dprintf(CRITICAL, "unknow boot_part (%d), using first boot partition\n", boot_part);
+ bootimg.part_name = (char *)BOOT_PART_NAME;
+ tz.part_name = (char *)TZ_PART_NAME;
+ }
+#endif
+
+ bootimg.recovery_part_name = (char *)RECOVERY_BOOT_PART_NAME;
+ bootimg.boot_mode = boot_mode;
+ bootimg.buf = kernel_buf;
+ bootimg_t = thread_create("bootimg_ctl", fit_load_thread, &bootimg,
+ DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+
+ /* create a tz thread to load tz */;
+ tz.recovery_part_name = (char *)RECOVERY_TZ_PART_NAME;
+ tz.boot_mode = boot_mode;
+ tz.buf = tz_buf;
+ tz_t = thread_create("tz_ctl", fit_load_thread, &tz,
+ DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+ if (!bootimg_t || !tz_t) {
+ dprintf(CRITICAL, "create load threads failed\n");
+ return;
+ }
+
+ thread_resume(bootimg_t);
+ thread_resume(tz_t);
+
+ thread_join(bootimg_t, &ret_bootimg, INFINITE_TIME);
+ thread_join(tz_t, &ret_tz, INFINITE_TIME);
+
+ if (ret_bootimg) {
+ dprintf(CRITICAL, "load boot image failed\n");
+ goto FASTBOOT;
+ }
+
+ if (ret_tz) {
+ dprintf(CRITICAL, "load tz image failed\n");
+ goto FASTBOOT;
+ }
+
+ plat_fixup_hook((void *)bootimg.dtb_load);
+
+ dtbo_buf = mempool_alloc(MAX_DTBO_SIZE, MEMPOOL_ANY);
+ if (!dtbo_buf) {
+ dprintf(CRITICAL, "alloc dtbo buf fail\n");
+ return;
+ }
+
+#ifdef AB_UPGRADE_APP
+ /*2.overlay cmdline to choose A/B rootfs*/
+ if (boot_part == 1) {
+ dprintf(CRITICAL, "load second partitions, need to overlay cmdline\n");
+ cmdlineoverlay((void *)bootimg.dtb_load, NULL, 0);
+ }
+#endif
+
+#ifdef SET_FDT_EMI_INFO
+ /* set fdt emi info*/
+ ret = extract_fdt((void *)bootimg.dtb_load, MAX_DTB_SIZE);
+ if (ret) {
+ dprintf(CRITICAL, "extract fdt failed\n");
+ return;
+ }
+
+ ret = set_fdt_emi_info((void *)bootimg.dtb_load);
+ if (ret < 0) {
+ dprintf(CRITICAL, "failed to set fdt emi info\n");
+ }
+
+ ret = fdt_pack((void *)bootimg.dtb_load);
+ if (ret) {
+ dprintf(CRITICAL, "ft pack failed\n");
+ return;
+ }
+#endif
+
+ /* check if dtbo is existed */
+ ret = fit_get_image(DTBO_PART_NAME, &dtbo_buf);
+ if (ret == 0) {
+ void *fdt_dtbo;
+ void *fdt_dtb;
+
+ if (bootimg.dtb_load == ERR_ADDR) {
+ dprintf(CRITICAL, "dtbo failed, no dtb\n");
+ return;
+ }
+ fdt_dtb = (void *)bootimg.dtb_load;
+
+ /* extract fdt */
+ ret = extract_fdt(fdt_dtb, MAX_DTB_SIZE);
+ if (ret) {
+ dprintf(CRITICAL, "extract fdt failed\n");
+ return;
+ }
+
+ dprintf(ALWAYS, "[fitboot] do overlay\n");
+ fdt_dtbo = (void *)dtbo_buf;
+ ret = fdt_overlay_apply(fdt_dtb, fdt_dtbo);
+ if (ret) {
+ dprintf(CRITICAL, "fdt merge failed, ret %d\n", ret);
+ return;
+ }
+
+ /* pack fdt */
+ ret = fdt_pack(fdt_dtb);
+ if (ret) {
+ dprintf(CRITICAL, "ft pack failed\n");
+ return;
+ }
+ }
+
+ /* load sspm*/
+ extern void fit_load_sspm_image() __attribute__((weak));
+ if (fit_load_sspm_image)
+ fit_load_sspm_image();
+
+ /*load spmfw*/
+ extern void fit_load_spmfw_image() __attribute__((weak));
+ if (fit_load_spmfw_image)
+ fit_load_spmfw_image();
+
+ /* load bl33 for tz to jump*/
+#if WITH_KERNEL_VM
+ addr_t fdt_pa = kvaddr_to_paddr((void *)bootimg.dtb_load);
+#else
+ addr_t fdt_pa = bootimg.dtb_load;
+#endif
+ setup_bl33(bl33, fdt_pa, (uint)(bootimg.kernel_entry));
+ memmove((void *)CFG_BL33_LOAD_EP_ADDR, bl33, sizeof(bl33));
+
+ ulong bl33_pa = CFG_BL33_LOAD_EP_ADDR;
+ ulong smc_fid = 0xc2000000UL; /* only used in ARCH_ARM64 */
+
+#if ARCH_ARM64 && WITH_KERNEL_VM
+ /* 64-bit LK use non identity mapping VA, VA to PA translation needed */
+ bl33_pa = (ulong)kvaddr_to_paddr((void *)CFG_BL33_LOAD_EP_ADDR);
+#endif
+ dprintf(ALWAYS, "LK run time: %lld (us)\n", current_time_hires());
+ dprintf(ALWAYS, "jump to tz %p\n", (void *)tz.kernel_entry);
+ arch_chain_load((void *)prepare_bl2_exit, smc_fid, tz.kernel_entry, bl33_pa, 0UL);
+}
+
+APP_START(fitboot)
+.entry = fitboot_task,
+ .flags = 0,
+ APP_END