/*
 * iap_jump.c
 *
 *  Created on: Oct 23, 2023
 *      Author: Warner
 */

#include <Loader/iap_jump.h>
#include <stdio.h>
#include <stdint.h>
#include <stdbool.h>

#include "LPC54114_cm4.h"




/*******************************************************************************
 * Definitions
 ******************************************************************************/
typedef void (*IAP_JumpFun)(void);


/*******************************************************************************
 * Variables
 ******************************************************************************/



/*******************************************************************************
 * Static Function Declarations
 ******************************************************************************/


/*******************************************************************************
 * Static Functions
 ******************************************************************************/

/*******************************************************************************
 * Public Functions
 ******************************************************************************/
#if 0
void IAP_JumpToAPP(uint32_t applicationAddress, uint32_t stackPointer){
	/* Static variables are needed as we need to ensure the values we are using are not stored on the
	previous stack */
	static uint32_t s_stackPointer = 0;
	s_stackPointer = stackPointer;
	static void (*farewellBootloader)(void) = 0;
	farewellBootloader = (void (*)(void))applicationAddress;

	/* Set the VTOR to the application vector table address */
	SCB->VTOR = applicationAddress;

	/* Set stack pointers to the application stack pointer */
	__set_MSP(s_stackPointer);
	__set_PSP(s_stackPointer);
	
	/* Jump to the application */
	farewellBootloader();
}
#endif

void IAP_JumpToApp(uint32_t app_addr){
	/* Static variables are needed as we need to ensure the values we are using are not stored on the
	previous stack */
	static uint32_t s_stackPointer = 0;
	s_stackPointer = app_addr;

	IAP_JumpFun JumpToApp;
	JumpToApp=(IAP_JumpFun)*(uint32_t*)(app_addr+4);

	/* Set the VTOR to the application vector table address */
	SCB->VTOR = app_addr;

	/* Set stack pointers to the application stack pointer */
	__set_MSP(*(uint32_t*)s_stackPointer);
	__set_PSP(*(uint32_t*)s_stackPointer);

	/* Jump to the application */
	JumpToApp();
}