/*********************************************************************
*                    SEGGER Microcontroller GmbH                     *
*                        The Embedded Experts                        *
**********************************************************************

-------------------------- END-OF-HEADER -----------------------------

File    : main.c
Purpose : Generic application start

*/

#include <stdio.h>
#include "em_device.h"

/// Bare boot table. Can be mapped on top of vector table to access contents.
typedef struct {
  /// Pointer to top of stack
  uint32_t *stackTop;
  /// Pointer to reset vector
  void     (*resetVector)(void);
  /// Reserved pointers to fault handlers
  uint32_t reserved0[5];
  /// Reserved pointers to RESERVED fields
  uint32_t reserved1[3];
  /// Pointer to bootloader table
  void     *table;
  /// Reserved pointers to SVC and DebugMon interrupts
  uint32_t reserved2[2];
  /// Pointer to application signature
  void     *signature;
} BareBootTable_t;

#define BTL_FIRST_STAGE_BASE              FLASH_BASE
#define BTL_APPLICATION_BASE              (FLASH_BASE + 0x00006000UL)




__attribute__ ((noreturn)) static void jumpToApplicationRoutine(uint32_t startOfAppSpace);
//static void bootToApp(uint32_t startOfAppSpace);
/*********************************************************************
*
*       main()
*
*  Function description
*   Application entry point.
*/
int btl_main(void) {

  int i;

  printf("Running the bootloader!\n");

  for (i = 0; i < 100; i++) {
    printf("Hello World %d!\n", i);
  }

  //TODO trigger reset

 
  do {
    i++;
  } while (1);
}

/**
 * Jump to app
 */
 /*
 __attribute__ ((noreturn, naked)) static void bootToApp(uint32_t startOfAppSpace)
{
  jumpToApplicationRoutine(startOfAppSpace);
  while (1) {
    // Do nothing
  }
}
*/
__attribute__ ((noreturn)) static inline void jumpToApplicationRoutine(uint32_t startOfAppSpace)
{
#if defined(BOOTLOADER_SECURE)

// Erase rw section
  __ASM volatile(
    "ldr r3,=0                  \n"
    "mov r1, %[rwStart]         \n" // Load start address
    "mov r2, %[rwEnd]           \n" // Load end address
#if defined(__GNUC__)
    "mov r4, %[bssStart]        \n" // Load bss section start address
    "mov r5, %[bssEnd]          \n" // Load bss section end address
#endif
    "data_clean_up_loop:        \n"
    "cmp r1, r2                 \n" // Check if we have reached the end address
    "beq data_clean_up_finished \n"
    "str r3, [r1]               \n" // Clear the RAM content pointed by R1
    "add r1, r1, #4             \n" // Increment by 4 to get to the next word address
    "b data_clean_up_loop       \n"
    "data_clean_up_finished:    \n"
#if defined(__GNUC__)
    "bss_clean_up_loop:         \n"
    "cmp r4, r5                 \n" // Check if we have reached the end address
    "beq bss_clean_up_finished  \n"
    "str r3, [r4]               \n" // Clear the RAM content pointed by R4
    "add r4, r4, #4             \n" // Increment by 4 to get to the next word address
    "b bss_clean_up_loop        \n"
    "bss_clean_up_finished:     \n"
#endif
    : //No outputs
#if defined(__GNUC__)
    :[rwStart] "r" (&__data_start__), [rwEnd] "r" (&__data_end__), [bssStart] "r" (&__bss_start__), [bssEnd] "r" (&__bss_end__) //Inputs
    : "r1", "r2", "r3", "r4", "r5" //Clobbered registers
    );
#else
    :[rwStart] "r" (&STACKSEAL$$Limit), [rwEnd] "r" (&HEAP$$Base) //Inputs
    : "r1", "r2", "r3" //Clobbered registers
    );
#endif

#if defined(TEST_BOOTLOADER_RAM_CLEAN_UP)
  __ASM volatile(
    "mov r5, %[dataSection]       \n"     // Address of the last word in the rw section
    "mov r6, %[stackSection]      \n"     // Address of the last word in the stack section
    "mov r7, #0xFEEB              \n"     // Magic word stored at the end of data and stack sections
    "sub r5, r5, #4               \n"
    "sub r6, r6, #4               \n"
    "str r7, [r5]                 \n"     // Store magic word at the end of data section
    "str r7, [r6]                 \n"     // Store magic word at the end of stack section
    : //No outputs
#if defined(__GNUC__)
    :[dataSection] "r" (&__bss_end__), [stackSection] "r" (&__StackTop) //Inputs
    : "r5", "r6", "r7", "r3" //Clobbered registers
    );
#else
    :[dataSection] "r" (&HEAP$$Base), [stackSection] "r" (&CSTACK$$Limit) //Inputs
    : "r5", "r6", "r7", "r3" //Clobbered registers
    );
#endif
#endif  // end of TEST_BOOTLOADER_RAM_CLEAN_UP

#endif // end of BOOTLOADER_SECURE

#if defined(BOOTLOADER_SECURE)
  // We should not leave any traces of the secure bootloader before jumping into the application
  // clean up entire RAM content except for the reset reason word
  __ASM volatile(
    "ldr r4,=0                 \n"
    "mov r2, %[stackBase]      \n" // Load start address
    "mov r3, %[stackTop]       \n" // Load end address
#if defined(TEST_BOOTLOADER_RAM_CLEAN_UP)
    "sub r3, r3, #4            \n" // Account for magic word at the top of stack section
#endif
    "mov r1, %[ramBase]        \n"
    "cmp r2, r1                \n" // Check if stack base is equal to SRAM_BASE
    "bne ram_clean_up_loop     \n"
    "add r2, r2, #4            \n" // Increment by 4 to omit reset reason word in SRAM
    "ram_clean_up_loop:        \n"
    "cmp r2, r3                \n" // Check if we have reached the end address
    "beq ram_clean_up_finished \n"
    "str r4, [r2]              \n" // Clear the RAM content pointed by R2
    "add r2, r2, #4            \n" // Increment by 4 to get to the next word address
    "b ram_clean_up_loop       \n"
    "ram_clean_up_finished:    \n"
#else
  // Load SP and PC of application
  __ASM volatile(
#endif // BOOTLOADER_SECURE
    "mov r0, %[startOfAppSpace]       \n" // Load address of SP into R0
    "ldr r1, [r0]                     \n" // Load SP into R1
    "msr msp, r1                      \n" // Set MSP
    "msr psp, r1                      \n" // Set PSP
    "ldr r0, [r0, #4]                 \n" // Load PC into R0
    "mov pc, r0                       \n" // Set PC
    : // No outputs
#if defined(BOOTLOADER_SECURE)
#if defined(__GNUC__)
    :[startOfAppSpace] "r" (startOfAppSpace), [stackBase] "r" (&__StackLimit), [stackTop] "r" (&__StackTop), [ramBase] "r" (SRAM_BASE)  // Inputs
    : "r0", "r1", "r2", "r3", "r4" // Clobbered registers
    );
#else
    :[startOfAppSpace] "r" (startOfAppSpace), [stackBase] "r" (&CSTACK$$Base), [stackTop] "r" (&CSTACK$$Limit), [ramBase] "r" (SRAM_BASE)
    : "r0", "r1", "r2", "r3", "r4" // Clobbered registers
    );
#endif // __GNUC__
#else
    :[startOfAppSpace] "r" (startOfAppSpace)  // Inputs
    : "r0", "r1" // Clobbered registers
    );
#endif  // BOOTLOADER_SECURE
  while (1) {
    // Do nothing
  }
}


/**************************************************************************//**
 * @brief
 *   Initialize the system.
 *
 * @details
 *   Do required generic HW system init.
 *
 * @note
 *   This function is invoked during system init, before the main() routine
 *   and any data has been initialized. For this reason, it cannot do any
 *   initialization of variables etc.
 *****************************************************************************/
void btl_init(void)
{
    BareBootTable_t* startOfAppSpace = (BareBootTable_t *)BTL_APPLICATION_BASE;

    // does application exist
    uint32_t pc = *(uint32_t *)(startOfAppSpace + 4);
    if (pc == 0xFFFFFFFF) {
         printf("no data in application image!\n");
         return;
    }

    SCB->VTOR = (uint32_t)startOfAppSpace;

    jumpToApplicationRoutine((uint32_t)startOfAppSpace);

}
/*************************** End of file ****************************/
