/*
 * Copyright (c) 2012-2014 Wind River Systems, Inc.
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <zephyr.h>
#include <sys/printk.h>

// register addresses per the nrf52 documentation
#define POWER_REG_BASE      0x40000000
#define GPREGRET            (POWER_REG_BASE + 0x51c)    // retention register
#define RESETREAS           (POWER_REG_BASE + 0x400)    // reset reason

#define IMAGE1_OFFSET 0x6000
#define IMAGE2_OFFSET 0x14000

typedef void *ADDR;

#define IMAGE1 ((ADDR)IMAGE1_OFFSET)   // 0x6000
#define IMAGE2 ((ADDR)IMAGE2_OFFSET)   // 0x14000

static __inline
void jump_to_addr( ADDR new_msp, ADDR new_lr, ADDR addr )
{
    __asm volatile ("MSR MSP, %[arg]" : : [arg] "r" (new_msp));
    __asm volatile ("MOV LR,  %[arg]" : : [arg] "r" (new_lr) : "lr");
    __asm volatile ("BX       %[arg]" : : [arg] "r" (addr));
}

void launch_image( ADDR *vector_table )
{
    ADDR new_msp         = vector_table[0];     // Stack Pointer
    ADDR reset_handler   = vector_table[1];     // Reset Handler
    ADDR new_lr          = (ADDR)0xFFFFFFFF;

    printk( "new_msp %08x\n", new_msp );
    printk( "new_lr %08x\n", new_lr );
    printk( "reset_handler %08x\n", reset_handler );
    jump_to_addr( new_msp, new_lr, reset_handler ); // Jump directly to the App's Reset Handler.
}

void main()
{   
    ADDR vector = NULL;
    uint32_t *reg = (uint32_t *)GPREGRET;

    if( *reg == 1 )
        vector = IMAGE1;
    else
        vector = IMAGE2;

     //reset the register
    *reg = 0;

    printk( "booting vector %p\n", vector );
    launch_image( vector );
}