startup.c

// TeensyCNC
// Copyright 2016 Matt Williams
//
// ARM startup and basic hardware Initialization

#include "MK20D10.h"
#include <string.h>
#include <stdlib.h>

extern char __stack[];

void __thumb_startup(void);

__attribute__((interrupt)) void Cpu_Interrupt(void)
{
}

volatile uint32_t Tick = 0;

__attribute__((interrupt)) void SysTick_IRQHandler(void)
{
    Tick++;
}

extern void __attribute__((interrupt)) Cpu_ivINT_FTM1(void);

extern void __attribute__((interrupt)) Cpu_ivINT_PORTB(void);

extern void __attribute__((interrupt)) Cpu_ivINT_PORTC(void);

extern void __attribute__((interrupt)) usb_isr(void);

// Interrupt vector table
__attribute__((used, section(".isr_vector"))) void(*const __vect_table[]) (void) = {
    (void *) &__stack,  //	0x00	ivINT_Initial_Stack_Pointer
    __thumb_startup,    //	0x01	ivINT_Initial_Program_Counter
    Cpu_Interrupt,      //	0x02	ivINT_NMI
    Cpu_Interrupt,      //	0x03	ivINT_Hard_Fault
    Cpu_Interrupt,      //	0x04	ivINT_Mem_Manage_Fault
    Cpu_Interrupt,      //	0x05	ivINT_Bus_Fault
    Cpu_Interrupt,      //	0x06	ivINT_Usage_Fault
    Cpu_Interrupt,      //	0x07	ivINT_Reserved7
    Cpu_Interrupt,      //	0x08	ivINT_Reserved8
    Cpu_Interrupt,      //	0x09	ivINT_Reserved9
    Cpu_Interrupt,      //	0x0A	ivINT_Reserved10
    Cpu_Interrupt,      //	0x0B	ivINT_SVCall
    Cpu_Interrupt,      //	0x0C	ivINT_DebugMonitor
    Cpu_Interrupt,      //	0x0D	ivINT_Reserved13
    Cpu_Interrupt,      //	0x0E	ivINT_PendableSrvReq
    SysTick_IRQHandler, //	0x0F	ivINT_SysTick
    Cpu_Interrupt,      //	0x10	ivINT_DMA0
    Cpu_Interrupt,      //	0x11	ivINT_DMA1
    Cpu_Interrupt,      //	0x12	ivINT_DMA2
    Cpu_Interrupt,      //	0x13	ivINT_DMA3
    Cpu_Interrupt,      //	0x14	ivINT_DMA4
    Cpu_Interrupt,      //	0x15	ivINT_DMA5
    Cpu_Interrupt,      //	0x16	ivINT_DMA6
    Cpu_Interrupt,      //	0x17	ivINT_DMA7
    Cpu_Interrupt,      //	0x18	ivINT_DMA8
    Cpu_Interrupt,      //	0x19	ivINT_DMA9
    Cpu_Interrupt,      //	0x1A	ivINT_DMA10
    Cpu_Interrupt,      //	0x1B	ivINT_DMA11
    Cpu_Interrupt,      //	0x1C	ivINT_DMA12
    Cpu_Interrupt,      //	0x1D	ivINT_DMA13
    Cpu_Interrupt,      //	0x1E	ivINT_DMA14
    Cpu_Interrupt,      //	0x1F	ivINT_DMA15
    Cpu_Interrupt,      //	0x20	ivINT_DMA_Error
    Cpu_Interrupt,      //	0x21	ivINT_MCM
    Cpu_Interrupt,      //	0x22	ivINT_FTFL
    Cpu_Interrupt,      //	0x23	ivINT_Read_Collision
    Cpu_Interrupt,      //	0x24	ivINT_LVD_LVW
    Cpu_Interrupt,      //	0x25	ivINT_LLW
    Cpu_Interrupt,      //	0x26	ivINT_Watchdog
    Cpu_Interrupt,      //	0x27	ivINT_Reserved39
    Cpu_Interrupt,      //	0x28	ivINT_I2C0
    Cpu_Interrupt,      //	0x29	ivINT_I2C1
    Cpu_Interrupt,      //	0x2A	ivINT_SPI0
    Cpu_Interrupt,      //	0x2B	ivINT_SPI1
    Cpu_Interrupt,      //	0x2C	ivINT_Reserved44
    Cpu_Interrupt,      //	0x2D	ivINT_CAN0_ORed_Message_buffer
    Cpu_Interrupt,      //	0x2E	ivINT_CAN0_Bus_Off
    Cpu_Interrupt,      //	0x2F	ivINT_CAN0_Error
    Cpu_Interrupt,      //	0x30	ivINT_CAN0_Tx_Warning
    Cpu_Interrupt,      //	0x31	ivINT_CAN0_Rx_Warning
    Cpu_Interrupt,      //	0x32	ivINT_CAN0_Wake_Up
    Cpu_Interrupt,      //	0x33	ivINT_I2S0_Tx
    Cpu_Interrupt,      //	0x34	ivINT_I2S0_Rx
    Cpu_Interrupt,      //	0x35	ivINT_Reserved53
    Cpu_Interrupt,      //	0x36	ivINT_Reserved54
    Cpu_Interrupt,      //	0x37	ivINT_Reserved55
    Cpu_Interrupt,      //	0x38	ivINT_Reserved56
    Cpu_Interrupt,      //	0x39	ivINT_Reserved57
    Cpu_Interrupt,      //	0x3A	ivINT_Reserved58
    Cpu_Interrupt,      //	0x3B	ivINT_Reserved59
    Cpu_Interrupt,      //	0x3C	ivINT_UART0_LON
    Cpu_Interrupt,      //	0x3D	ivINT_UART0_RX_TX
    Cpu_Interrupt,      //	0x3E	ivINT_UART0_ERR
    Cpu_Interrupt,      //	0x3F	ivINT_UART1_RX_TX
    Cpu_Interrupt,      //	0x40	ivINT_UART1_ERR
    Cpu_Interrupt,      //	0x41	ivINT_UART2_RX_TX
    Cpu_Interrupt,      //	0x42	ivINT_UART2_ERR
    Cpu_Interrupt,      //	0x43	ivINT_UART3_RX_TX
    Cpu_Interrupt,      //	0x44	ivINT_UART3_ERR
    Cpu_Interrupt,      //	0x45	ivINT_UART4_RX_TX
    Cpu_Interrupt,      //	0x46	ivINT_UART4_ERR
    Cpu_Interrupt,      //	0x47	ivINT_Reserved71
    Cpu_Interrupt,      //	0x48	ivINT_Reserved72
    Cpu_Interrupt,      //	0x49	ivINT_ADC0
    Cpu_Interrupt,      //	0x4A	ivINT_ADC1
    Cpu_Interrupt,      //	0x4B	ivINT_CMP0
    Cpu_Interrupt,      //	0x4C	ivINT_CMP1
    Cpu_Interrupt,      //	0x4D	ivINT_CMP2
    Cpu_Interrupt,      //	0x4E	ivINT_FTM0
    Cpu_ivINT_FTM1,     //	0x4F	ivINT_FTM1
    Cpu_Interrupt,      //	0x50	ivINT_FTM2
    Cpu_Interrupt,      //	0x51	ivINT_CMT
    Cpu_Interrupt,      //	0x52	ivINT_RTC
    Cpu_Interrupt,      //	0x53	ivINT_RTC_Seconds
    Cpu_Interrupt,      //	0x54	ivINT_PIT0
    Cpu_Interrupt,      //	0x55	ivINT_PIT1
    Cpu_Interrupt,      //	0x56	ivINT_PIT2
    Cpu_Interrupt,      //	0x57	ivINT_PIT3
    Cpu_Interrupt,      //	0x58	ivINT_PDB0
    usb_isr,            //	0x59	ivINT_USB0
    Cpu_Interrupt,      //	0x5A	ivINT_USBDCD
    Cpu_Interrupt,      //	0x5B	ivINT_Reserved91
    Cpu_Interrupt,      //	0x5C	ivINT_Reserved92
    Cpu_Interrupt,      //	0x5D	ivINT_Reserved93
    Cpu_Interrupt,      //	0x5E	ivINT_Reserved94
    Cpu_Interrupt,      //	0x5F	ivINT_Reserved95
    Cpu_Interrupt,      //	0x60	ivINT_Reserved96
    Cpu_Interrupt,      //	0x61	ivINT_DAC0
    Cpu_Interrupt,      //	0x62	ivINT_Reserved98
    Cpu_Interrupt,      //	0x63	ivINT_TSI0
    Cpu_Interrupt,      //	0x64	ivINT_MCG
    Cpu_Interrupt,      //	0x65	ivINT_LPTimer
    Cpu_Interrupt,      //	0x66	ivINT_Reserved102
    Cpu_Interrupt,      //	0x67	ivINT_PORTA
    Cpu_ivINT_PORTB,    //	0x68	ivINT_PORTB
    Cpu_ivINT_PORTC,    //	0x69	ivINT_PORTC
    Cpu_Interrupt,      //	0x6A	ivINT_PORTD
    Cpu_Interrupt,      //	0x6B	ivINT_PORTE
    Cpu_Interrupt,      //	0x6C	ivINT_Reserved108
    Cpu_Interrupt,      //	0x6D	ivINT_Reserved109
    Cpu_Interrupt       //	0x6E	ivINT_SWI
};

// Flash configuration field
__attribute__((section(".FlashConfig"))) const uint8_t _cfm[0x10] = {
    0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x7E, 0xFB, 0xFF, 0xFF
};

extern int main(void);

// format of the ROM table info entry
typedef struct RomInfo
{
    unsigned long Source;
    unsigned long Target;
    unsigned long Size;
} RomInfo;

// linker defined symbol
extern RomInfo __S_romp[] __attribute__((weak));

// Routine to copy a single section from ROM to RAM
void __copy_rom_section(unsigned long dst, unsigned long src, unsigned long size)
{
    const int size_int   = sizeof(int);
    const int mask_int   = sizeof(int) - 1;
    const int size_short = sizeof(short);
    const int mask_short = sizeof(short) - 1;
    const int size_char  = sizeof(char);
    unsigned long len    = size;

    if (dst == src || size == 0)
    {
        return;
    }
    while (len > 0)
    {
        if (!(src & mask_int) && !(dst & mask_int) && len >= size_int)
        {
            *((int *) dst) = *((int *) src);
            dst += size_int;
            src += size_int;
            len -= size_int;
        }
        else
        if (!(src & mask_short) && !(dst & mask_short) && len >= size_short)
        {
            *((short *) dst) = *((short *) src);
            dst += size_short;
            src += size_short;
            len -= size_short;
        }
        else
        {
            *((char *) dst) = *((char *) src);
            dst += size_char;
            src += size_char;
            len -= size_char;
        }
    }
} /* __copy_rom_section */

// Routine that copies all sections the user marked as ROM into their target RAM addresses.
// __S_romp is defined in the linker command file, it is a table of RomInfo structures.
// The final entry in the table has all-zero fields.
void __copy_rom_sections_to_ram(void)
{
    int index;

    if (__S_romp == 0)
    {
        return;
    }

    // Go through the entire table, copying sections from ROM to RAM.
    for (index = 0; __S_romp[index].Source != 0 || __S_romp[index].Target != 0 || __S_romp[index].Size != 0; ++index)
        __copy_rom_section(__S_romp[index].Target, __S_romp[index].Source, __S_romp[index].Size);
}

void __zero_fill_bss(void)
{
    extern char __START_BSS[];
    extern char __END_BSS[];
    unsigned long len    = __END_BSS - __START_BSS;
    unsigned long dst    = (unsigned long) __START_BSS;
    const int size_int   = sizeof(int);
    const int mask_int   = sizeof(int) - 1;
    const int size_short = sizeof(short);
    const int mask_short = sizeof(short) - 1;
    const int size_char  = sizeof(char);

    if (len == 0)
    {
        return;
    }
    while (len > 0)
    {
        if (!(dst & mask_int) && len >= size_int)
        {
            *((int *) dst) = 0;
            dst += size_int;
            len -= size_int;
        }
        else
        if (!(dst & mask_short) && len >= size_short)
        {
            *((short *) dst) = 0;
            dst += size_short;
            len -= size_short;
        }
        else
        {
            *((char *) dst) = 0;
            dst += size_char;
            len -= size_char;
        }
    }
} /* __zero_fill_bss */

void __init_hardware(void)
{
    // Set the interrupt vector table position
    SCB->VTOR = (uint32_t) (&__vect_table);
    // Disable the WDOG module
    WDOG->UNLOCK  = WDOG_UNLOCK_WDOGUNLOCK(0xC520U);
    WDOG->UNLOCK  = WDOG_UNLOCK_WDOGUNLOCK(0xD928U);
    WDOG->STCTRLH = WDOG_STCTRLH_BYTESEL(0x00U) | WDOG_STCTRLH_WAITEN_MASK | WDOG_STCTRLH_STOPEN_MASK
      | WDOG_STCTRLH_ALLOWUPDATE_MASK | WDOG_STCTRLH_CLKSRC_MASK | 0x0100U;
    // System clock initialization

    // Set the system prescalers to safe value
    SIM->CLKDIV1 = SIM_CLKDIV1_OUTDIV1(0x00) | SIM_CLKDIV1_OUTDIV2(0x01) | SIM_CLKDIV1_OUTDIV3(0x03)
      | SIM_CLKDIV1_OUTDIV4(0x03);
    // Enable clock gate for ports to enable pin routing
    SIM->SCGC5 |= SIM_SCGC5_PORTD_MASK | SIM_SCGC5_PORTC_MASK | SIM_SCGC5_PORTB_MASK | SIM_SCGC5_PORTA_MASK;

    // Release IO pads after wakeup from VLLS mode.
    if ((PMC->REGSC & PMC_REGSC_ACKISO_MASK) != 0)
    {
        PMC->REGSC |= PMC_REGSC_ACKISO_MASK;
    }
    // Update system prescalers
    // enable capacitors for crystal
    OSC->CR = OSC_CR_SC8P_MASK | OSC_CR_SC2P_MASK | OSC_CR_ERCLKEN_MASK;
    // enable osc, 8-32 MHz range, low power mode
    MCG->C2 = MCG_C2_RANGE0(2) | MCG_C2_EREFS_MASK;
    // switch to crystal as clock source, FLL input = 16 MHz / 512
    MCG->C1 = MCG_C1_CLKS(2) | MCG_C1_FRDIV(4);
    // wait for crystal oscillator to begin
    while ((MCG->S & MCG_S_OSCINIT0_MASK) == 0)
        ;
    // wait for FLL to use oscillator
    while ((MCG->S & MCG_S_IREFST_MASK) != 0)
        ;
    // wait for MCGOUT to use oscillator
    while ((MCG->S & MCG_S_CLKST_MASK) != MCG_S_CLKST(2))
        ;

    // now in FBE mode
    //  C1[CLKS] bits are written to 10
    //  C1[IREFS] bit is written to 0
    //  C1[FRDIV] must be written to divide xtal to 31.25-39 kHz
    //  C6[PLLS] bit is written to 0
    //  C2[LP] is written to 0
    // if we need faster than the crystal, turn on the PLL
    MCG->C5 = MCG_C5_PRDIV0(5);                   // config PLL input for 16 MHz Crystal / 6 = 2.667 Hz
    MCG->C6 = MCG_C6_PLLS_MASK | MCG_C6_VDIV0(3); // config PLL for 72 MHz output

    // wait for PLL to start using xtal as its input
    while (!(MCG->S & MCG_S_PLLST_MASK))
        ;
    // wait for PLL to lock
    while (!(MCG->S & MCG_S_LOCK0_MASK))
        ;
    // now we're in PBE mode

    // now program the clock dividers
    SIM->CLKDIV1 = SIM_CLKDIV1_OUTDIV1(0) | SIM_CLKDIV1_OUTDIV2(1) | SIM_CLKDIV1_OUTDIV4(2);
    SIM->CLKDIV2 = SIM_CLKDIV2_USBDIV(2) | SIM_CLKDIV2_USBFRAC_MASK;

    // switch to PLL as clock source, FLL input = 16 MHz / 512
    MCG->C1 = MCG_C1_CLKS(0) | MCG_C1_FRDIV(4);
    // wait for PLL clock to be used
    while ((MCG->S & MCG_S_CLKST_MASK) != MCG_S_CLKST(3))
        ;
    // now we're in PEE mode
    // trace is CPU clock, CLKOUT=OSCERCLK0
    // USB uses PLL clock
    SIM->SOPT2 = SIM_SOPT2_USBSRC_MASK | SIM_SOPT2_PLLFLLSEL_MASK | SIM_SOPT2_CLKOUTSEL(6);
} /* __init_hardware */

void Cpu_SetBASEPRI(uint32_t Level)
{
    __asm__ volatile ("msr basepri, %[input]" ::[input] "r" (Level) :);
}

void __low_level_init(void)
{
    /* Initialization of the RCM module */
    RCM->RPFW &= ~(RCM_RPFW_RSTFLTSEL(0x1F));
    RCM->RPFC &= ~(RCM_RPFC_RSTFLTSS_MASK | RCM_RPFC_RSTFLTSRW(0x03));

    /* Initialization of the PMC module */
    PMC->LVDSC1 = (PMC->LVDSC1 & ~(PMC_LVDSC1_LVDIE_MASK | PMC_LVDSC1_LVDV(0x03))) | (PMC_LVDSC1_LVDACK_MASK
      | PMC_LVDSC1_LVDRE_MASK);
    PMC->LVDSC2 = (PMC->LVDSC2 & ~(PMC_LVDSC2_LVWIE_MASK | PMC_LVDSC2_LVWV(0x03))) | PMC_LVDSC2_LVWACK_MASK;
    PMC->REGSC &= ~(PMC_REGSC_BGEN_MASK | PMC_REGSC_ACKISO_MASK | PMC_REGSC_BGBE_MASK);
    SMC->PMPROT = 0x00;
    LLWU->RST   = LLWU_RST_LLRSTE_MASK;
    NVIC_SetPriority(USB0_IRQn, 0x00);
    NVIC_SetPriority(LVD_LVW_IRQn, 0x00);
    GPIOB->PDDR &= ~GPIO_PDDR_PDD(0x0b);
    GPIOC->PDDR &= ~GPIO_PDDR_PDD(0xd0);

    // Set up SysTick
    SysTick->LOAD = (72000000 / 1000000) - 1;
    SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | SysTick_CTRL_TICKINT_Msk | SysTick_CTRL_ENABLE_Msk;

    // Enable interrupts of the given priority level
    Cpu_SetBASEPRI(0);
    __enable_irq();
}

void __thumb_startup(void)
{
    int addr = (int) __stack;

    // setup hardware
    __init_hardware();
    // setup the stack before we attempt anything else
    // skip stack setup if __stack is 0
    // assume sp is already setup.
    __asm(
        "mov r0,%0\n\t"
        "cmp r0,#0\n\t"
        "beq skip_sp\n\t"
        "mov sp,r0\n\t"
        "sub sp,#4\n\t"
        "mov r0,#0\n\t"
        "mvn r0,r0\n\t"
        "str r0,[sp,#0]\n\t"
        "add sp,#4\n\t"
        "skip_sp:\n\t"
        ::"r" (addr));

    // zero-fill the .bss section
    __zero_fill_bss();

    // SUPPORT_ROM_TO_RAM
    __copy_rom_sections_to_ram();
    __low_level_init();
    main();

    // should never get here
    while (1)
        ;
} /* __thumb_startup */