/****************************************************************************************//**
 *  @file       _start.c
 *
 *  @author     Fotis Panagiotopoulos
 *
 *  Startup code. _start should be called by the Reset Handler, and never return.
 *
 *******************************************************************************************/

#include "LPC17xx.h"
#include "hal.h"
#include "hal_config.h"
#include "interrupt.h"
#include "hal_types.h"
#include "hal_assert.h"

/** Begin address for the initialization values of the .data section. */
extern unsigned int __textdata__;
/** Begin address for the .data section. */
extern unsigned int __data_start__;
/** End address for the .data section. */
extern unsigned int __data_end__;
/** Start of the exception stack */
extern const unsigned int __exception_stack_start__;
/** End of the exception stack */
extern const unsigned int __exception_stack_end__;
/** Begin address for the .bss section. */
extern unsigned int __bss_start__;
/** End address for the .bss section */
extern unsigned int __bss_end__;


/** Main program entry point. */
extern int main(void);

/** Exit system call. */
extern void _exit(int code);

/** Initializes the data section. */
static void __attribute__((always_inline)) __initialize_data (unsigned int* from, unsigned int* region_begin, unsigned int* region_end);

/** Initializes the BSS section. */
static void __attribute__((always_inline)) __initialize_bss (unsigned int* region_begin, unsigned int* region_end);

/** Start-up code. */
void __attribute__ ((section(".after_vectors"), noreturn, used)) _start(void);


void _start (void)
{
    //Disable the interrupts, just to be safe. The system is not
    //yet ready to call functions.
    Interrupts_globalDisable();

    //If there is an exception/interrupt stack allocated,
    //then the stack pointers in the CPU have to be set
    //accordingly. Furthermore the CPU has to be turned to
    //thread (unprivileged) mode.
    if (&__exception_stack_end__ - &__exception_stack_start__ != 0)
    {
        asm volatile (
                        "ldr     r0, = __exception_stack_end__  \n\t" /* Load the exception stack top to register r0 */
                        "msr     MSP, r0                        \n\t" /* Move the exception stack top value to the MSP */
                        "isb                                    \n\t" /* Flush the pipeline */

                        "ldr     r0, = __stack                  \n\t" /* Load the process stack top to register r0 */
                        "msr     PSP, r0                        \n\t" /* Move the process stack top value to the PSP */
                        "movs    r0, #2                         \n\t" /* Load the constant 2 to register r0 */
                        "msr     CONTROL, r0                    \n\t" /* Move the constant to the CONTROL register,
                                                                        setting the CPU in non-privileged mode. */
                        "isb                                    \n\t" /* Flush the pipeline */

                        : : : "r0");
    }


    // Initialize hardware right after reset, to switch clock to higher
    // frequency and have the rest of the initializations run faster.
    SystemInit();


    // Copy the DATA segment from Flash to RAM (inlined).
    __initialize_data(&__textdata__, &__data_start__, &__data_end__);

    // Zero fill the BSS section (inlined).
    __initialize_bss(&__bss_start__, &__bss_end__);

    //Core is running normally, RAM is initialized properly,
    //now the system must be fully functional.

    //Set the default priority for all interrupts.
    for (int i = 0; i < 35; i++)  //The LPC1769 has 35 priorities, see the CMSIS file.
    {
        NVIC_SetPriority(i, INTERRUPT_DEFAULT_PRIORITY);
    }

    //Update the SystemCoreClock variable.
    SystemCoreClockUpdate();

    //Check the PLLs are working normally, and that the system
    //frequency is the expected.
    HAL_ASSERT(SystemCoreClock == CPU_FREQ);

    //Now that everything is up and running, enable the interrupts
    Interrupts_globalEnable();

    //NOTE!!! Until here the OS must NOT be running. Any OS related
    //functions must only be called within main!

    // Call the main entry point, and save the exit code.
    int code = main();


    //Main should never return. If it does, let the system exit gracefully.
    //Again, here it is assumed that the OS has stopped. The OS must be
    //functional only within main.
    _exit (code);

    // Should never reach this, _exit() should have already
    // performed a reset.
    while(1);
}

static inline void __initialize_data (unsigned int* from, unsigned int* region_begin, unsigned int* region_end)
{
    // Iterate and copy word by word.
    // It is assumed that the pointers are word aligned.
    unsigned int *p = region_begin;
    while (p < region_end)
        *p++ = *from++;
}

static inline void __initialize_bss (unsigned int* region_begin, unsigned int* region_end)
{
    // Iterate and clear word by word.
    // It is assumed that the pointers are word aligned.
    unsigned int *p = region_begin;
    while (p < region_end)
        *p++ = 0;
}