/**
 * Initialize the system
 *
 * @param  none
 * @return none
 *
 * @brief  Setup the microcontroller system
 *
 */

/*-----------------------------------------------------------------------------------
                                       INCLUDE HEADE FILES
 ------------------------------------------------------------------------------------*/
#include <stdio.h>

#include "xc6xxx.h"
#include "xc_drv_gpio.h"

/*------------------------------------------------------------------------------------
                                             Macros
 -------------------------------------------------------------------------------------*/
#define __DEBUG_OUT_PORT 0

/*------------------------------------------------------------------------------------
                                            Functions
 -------------------------------------------------------------------------------------*/



#define VECTOR_NUM 48

void set_vector(void)
{
#if (USE_XIP == 1)
    __disable_irq();
    for (uint32_t i = 0, *Pvector = (uint32_t *)(0x11001000 + 0),
                  *_vector_table = (uint32_t *)(0x10000000);
         i < VECTOR_NUM; i++) // copy vertor table
    {
        _vector_table[i] = *Pvector++;
    }
    __enable_irq();
    //inter vertor table remap
    *((volatile unsigned int*)(0x4000013C)) =0x10000001; 
#endif
}


static void WDT_ResetInit(void)
{
    cpr_ctlapbclken_grctl__wdt_pclk_en__setf(ENABLE);

    cpr_rstctl_ctlapb_sw__wdt_rstn__setf(RSTCTL_ENABLE);
    cpr_rstctl_ctlapb_sw__wdt_rstn__setf(RSTCTL_DISABLE);

    cpr_rstctl_wdtrst_mask_set(
        (WDT_SYS_RSTN_MASK_DISABLE | WDT_M0_RSTN_MASK_ENABLE));

    cpr_lp_ctl__ctl_wdt_tclk_en__setf(ENABLE);
    wdt_cr__wdt_en__setf(DISABLE);
}

void SystemInit(void)
{
    WDT_ResetInit();

#if (USE_XIP == 1)
    uint32_t *p=((unsigned int *)0x10000000);
    for(int i=0;i<1000;i++) {
        *(p++)=0;
    }  
    set_vector();
#endif 
}

//__RAM_CODE int sendchar(int c)
int sendchar(int c)
{
    unsigned int status;
#if (__DEBUG_OUT_PORT == 1)
    for (;;) {
        status  = (*((volatile unsigned *)(0x40011000 + 0x14)));
        status &= 0x20;
        if (status == 0x20)
            break;
    }
    (*((volatile unsigned *)(0x40011000 + 0x00))) = c;
    return (1);
#else
    for (;;) {
        status  = (*((volatile unsigned *)(0x40010000 + 0x14)));
        status &= 0x20;
        if (status == 0x20)
            break;
    }
    (*((volatile unsigned *)(0x40010000 + 0x00))) = c;
    return (1);
#endif
}

struct __FILE
{
    int handle; /* Add whatever you need here */
};

FILE __stdout;
__RAM_CODE int fputc(int ch, FILE *f) { return (sendchar(ch)); }

int ferror(FILE *f)
{
    /* Your implementation of ferror */
    return EOF;
}

void _ttywrch(int ch) { sendchar(ch); }

void _sys_exit(int return_code)
{
label:
    goto label; /* endless loop */
}