diff --git a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_hooks.c b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_hooks.c index 7e1f0665..8b59c2c2 100644 --- a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_hooks.c +++ b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_hooks.c @@ -97,3 +97,25 @@ u32 XFsbl_HookBeforeHandoff(void ) return Status; } + +/*****************************************************************************/ +/** + * This is a hook function where user can include the functionality to be run + * before FSBL fallback happens + * + * @param none + * + * @return error status based on implemented functionality (SUCCESS by default) + * + *****************************************************************************/ + +u32 XFsbl_HookBeforeFallback(void) +{ + u32 Status = XFSBL_SUCCESS; + + /** + * Add the code here + */ + + return Status; +} diff --git a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_hw.h b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_hw.h index 06ee54a6..dacddf4f 100644 --- a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_hw.h +++ b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_hw.h @@ -207,6 +207,12 @@ extern "C" { #define CRL_APB_BOOT_MODE_USER ( ( CRL_APB_BASEADDR ) + 0X00000200U ) #define CRL_APB_BOOT_MODE_USER_BOOT_MODE_MASK 0X0000000FU +/** + * Register: CRL_APB_RESET_CTRL + */ +#define CRL_APB_RESET_CTRL ( ( CRL_APB_BASEADDR ) + 0X00000218U ) +#define CRL_APB_RESET_CTRL_SOFT_RESET_MASK 0X00000010U + /* apu */ /** diff --git a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_main.c b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_main.c index 4c70c816..47234483 100644 --- a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_main.c +++ b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_main.c @@ -384,30 +384,58 @@ void XFsbl_ErrorLockDown(u32 ErrorStatus) * * @note We will not return from this function as it does soft reset *****************************************************************************/ -void XFsbl_FallBack(void ) +void XFsbl_FallBack(void) { + u32 RegValue; - /** - * Increment the Multiboot register - */ + /* Hook before FSBL Fallback */ + XFsbl_HookBeforeFallback(); - /** - * Hook before FSBL Fallback - */ + /* Read the Multiboot register */ + RegValue = XFsbl_In32(CSU_CSU_MULTI_BOOT); - /** - * dsb and isb to make sure every thing completes - */ + XFsbl_Printf(DEBUG_GENERAL,"Performing FSBL FallBack\n\r"); - /** - * Soft reset the system - */ + XFsbl_UpdateMultiBoot(RegValue+1); + + return; +} + + +/*****************************************************************************/ +/** + * This is the function which actually updates the multiboot register and + * does the soft reset. This function is called in fallback case and + * in the cases where user would like to jump to a different image, + * corresponding to the multiboot value being passed to this function. + * The latter case is a generic one and need arise because of error scenario. + * + * @param MultiBootValue is the new value for the multiboot register + * + * @return none + * + * @note We will not return from this function as it does soft reset + *****************************************************************************/ + +void XFsbl_UpdateMultiBoot(u32 MultiBootValue) +{ + u32 RegValue; + + XFsbl_Out32(CSU_CSU_MULTI_BOOT, MultiBootValue); + + /* make sure every thing completes */ + dsb(); + isb(); + + /* Soft reset the system */ + XFsbl_Printf(DEBUG_GENERAL,"Performing System Soft Reset\n\r"); + RegValue = XFsbl_In32(CRL_APB_RESET_CTRL); + XFsbl_Out32(CRL_APB_RESET_CTRL, + RegValue|CRL_APB_RESET_CTRL_SOFT_RESET_MASK); + + /* wait here until reset happens */ + while(1); + + return; - /** - * Fall back is not supported now, placing CPU in wfe state - * Exit FSBL - */ - XFsbl_HandoffExit(0U, XFSBL_NO_HANDOFFEXIT); - - return ; } diff --git a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_main.h b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_main.h index 878d639e..50fd8b7f 100644 --- a/lib/sw_apps/zynqmp_fsbl/src/xfsbl_main.h +++ b/lib/sw_apps/zynqmp_fsbl/src/xfsbl_main.h @@ -139,6 +139,7 @@ typedef struct { void XFsbl_PrintFsblBanner(void ); void XFsbl_ErrorLockDown(u32 ErrorStatus); void XFsbl_FallBack(void ); +void XFsbl_UpdateMultiBoot(u32 MultiBootValue); /** * Functions defined in xfsbl_initialization.c