Fix start on power up by providing Reset_Handler ourselves.

On power up the FlexRAM banks are in an unknown config so we can't
rely on the stack until after we configure FlexRAM.

10 files changed, 99 insertions, 210 deletions

diff --git a/ports/mimxrt10xx/Makefile b/ports/mimxrt10xx/Makefile
index d46806dc6..66a540a40 100644
--- a/ports/mimxrt10xx/Makefile
+++ b/ports/mimxrt10xx/Makefile
@@ -84,24 +84,10 @@ CFLAGS += -DCFG_TUSB_MCU=OPT_MCU_MIMXRT10XX -DCFG_TUD_MIDI_RX_BUFSIZE=128 -DCFG_
 ifeq ($(DEBUG), 1)
   CFLAGS += -ggdb
   # You may want to disable -flto if it interferes with debugging.
-  #CFLAGS += -flto -flto-partition=none
+  # CFLAGS += -flto -flto-partition=none
   # You may want to enable these flags to make setting breakpoints easier.
   CFLAGS += -fno-inline -fno-ipa-sra
 else
-  # -finline-limit can shrink the image size.
-  # -finline-limit=80 or so is similar to not having it on.
-  # There is no simple default value, though.
-
-  # Do a default shrink for small builds.
-  ifndef CFLAGS_INLINE_LIMIT
-    ifeq ($(CIRCUITPY_SMALL_BUILD),1)
-      CFLAGS_INLINE_LIMIT = 50
-    endif
-  endif
-
-  ifdef CFLAGS_INLINE_LIMIT
-    CFLAGS += -finline-limit=$(CFLAGS_INLINE_LIMIT)
-  endif
   #CFLAGS += -flto -flto-partition=none
 endif
 
@@ -116,12 +102,8 @@ CFLAGS += \
 	-DCPU_$(CHIP_VARIANT) \
 	-DDEBUG \
 	-DIMXRT10XX \
-	-DXIP_EXTERNAL_FLASH=1 \
-	-DXIP_BOOT_HEADER_ENABLE=1 \
-	-D__START=main \
 	-Os -g3 -Wno-unused-parameter \
-	-ffunction-sections -fdata-sections -fstack-usage \
-	-D__STARTUP_CLEAR_BSS
+	-ffunction-sections -fdata-sections -fstack-usage
 
 LD_FILES = $(wildcard boards/$(BOARD)/*.ld) $(addprefix linking/, flash/$(FLASH).ld chip_family/$(CHIP_FAMILY).ld common.ld)
 
diff --git a/ports/mimxrt10xx/boards/imxrt1010_evk/flash_config.c b/ports/mimxrt10xx/boards/imxrt1010_evk/flash_config.c
index c281d4541..805e62f01 100644
--- a/ports/mimxrt10xx/boards/imxrt1010_evk/flash_config.c
+++ b/ports/mimxrt10xx/boards/imxrt1010_evk/flash_config.c
@@ -13,12 +13,7 @@
 #define FSL_COMPONENT_ID "platform.drivers.xip_device"
 #endif
 
-#if defined(XIP_BOOT_HEADER_ENABLE) && (XIP_BOOT_HEADER_ENABLE == 1)
-#if defined(__CC_ARM) || defined(__ARMCC_VERSION) || defined(__GNUC__)
-    __attribute__((section(".boot_hdr.ivt")))
-#elif defined(__ICCARM__)
-#pragma location=".boot_hdr.ivt"
-#endif
+__attribute__((section(".boot_hdr.ivt")))
 /*************************************
  *  IVT Data
  *************************************/
@@ -33,11 +28,7 @@ const ivt image_vector_table = {
   IVT_RSVD                            /* Reserved = 0 */
 };
 
-#if defined(__CC_ARM) || defined(__ARMCC_VERSION) || defined(__GNUC__)
-    __attribute__((section(".boot_hdr.boot_data")))
-#elif defined(__ICCARM__)
-#pragma location=".boot_hdr.boot_data"
-#endif
+__attribute__((section(".boot_hdr.boot_data")))
 /*************************************
  *  Boot Data
  *************************************/
@@ -49,12 +40,7 @@ const BOOT_DATA_T boot_data = {
 };
 #endif
 
-#if defined(__CC_ARM) || defined(__ARMCC_VERSION) || defined(__GNUC__)
 __attribute__((section(".boot_hdr.conf")))
-#elif defined(__ICCARM__)
-#pragma location = ".boot_hdr.conf"
-#endif
-
 // Values copied from https://github.com/PaulStoffregen/cores/blob/ddb23fa5d97dac763bc06e11b9b41f026bd51f0a/teensy4/bootdata.c#L39
 const flexspi_nor_config_t qspiflash_config = {
     .memConfig =
diff --git a/ports/mimxrt10xx/boards/imxrt1060_evk/flash_config.c b/ports/mimxrt10xx/boards/imxrt1060_evk/flash_config.c
index c281d4541..c51c59a51 100644
--- a/ports/mimxrt10xx/boards/imxrt1060_evk/flash_config.c
+++ b/ports/mimxrt10xx/boards/imxrt1060_evk/flash_config.c
@@ -13,12 +13,8 @@
 #define FSL_COMPONENT_ID "platform.drivers.xip_device"
 #endif
 
-#if defined(XIP_BOOT_HEADER_ENABLE) && (XIP_BOOT_HEADER_ENABLE == 1)
-#if defined(__CC_ARM) || defined(__ARMCC_VERSION) || defined(__GNUC__)
-    __attribute__((section(".boot_hdr.ivt")))
-#elif defined(__ICCARM__)
-#pragma location=".boot_hdr.ivt"
-#endif
+__attribute__((section(".boot_hdr.ivt")))
+
 /*************************************
  *  IVT Data
  *************************************/
@@ -33,11 +29,7 @@ const ivt image_vector_table = {
   IVT_RSVD                            /* Reserved = 0 */
 };
 
-#if defined(__CC_ARM) || defined(__ARMCC_VERSION) || defined(__GNUC__)
-    __attribute__((section(".boot_hdr.boot_data")))
-#elif defined(__ICCARM__)
-#pragma location=".boot_hdr.boot_data"
-#endif
+__attribute__((section(".boot_hdr.boot_data")))
 /*************************************
  *  Boot Data
  *************************************/
@@ -49,12 +41,7 @@ const BOOT_DATA_T boot_data = {
 };
 #endif
 
-#if defined(__CC_ARM) || defined(__ARMCC_VERSION) || defined(__GNUC__)
 __attribute__((section(".boot_hdr.conf")))
-#elif defined(__ICCARM__)
-#pragma location = ".boot_hdr.conf"
-#endif
-
 // Values copied from https://github.com/PaulStoffregen/cores/blob/ddb23fa5d97dac763bc06e11b9b41f026bd51f0a/teensy4/bootdata.c#L39
 const flexspi_nor_config_t qspiflash_config = {
     .memConfig =
diff --git a/ports/mimxrt10xx/boards/teensy40/flash_config.c b/ports/mimxrt10xx/boards/teensy40/flash_config.c
index c281d4541..426deb884 100644
--- a/ports/mimxrt10xx/boards/teensy40/flash_config.c
+++ b/ports/mimxrt10xx/boards/teensy40/flash_config.c
@@ -13,12 +13,7 @@
 #define FSL_COMPONENT_ID "platform.drivers.xip_device"
 #endif
 
-#if defined(XIP_BOOT_HEADER_ENABLE) && (XIP_BOOT_HEADER_ENABLE == 1)
-#if defined(__CC_ARM) || defined(__ARMCC_VERSION) || defined(__GNUC__)
-    __attribute__((section(".boot_hdr.ivt")))
-#elif defined(__ICCARM__)
-#pragma location=".boot_hdr.ivt"
-#endif
+__attribute__((section(".boot_hdr.ivt")))
 /*************************************
  *  IVT Data
  *************************************/
@@ -33,11 +28,7 @@ const ivt image_vector_table = {
   IVT_RSVD                            /* Reserved = 0 */
 };
 
-#if defined(__CC_ARM) || defined(__ARMCC_VERSION) || defined(__GNUC__)
-    __attribute__((section(".boot_hdr.boot_data")))
-#elif defined(__ICCARM__)
-#pragma location=".boot_hdr.boot_data"
-#endif
+__attribute__((section(".boot_hdr.boot_data")))
 /*************************************
  *  Boot Data
  *************************************/
@@ -47,14 +38,8 @@ const BOOT_DATA_T boot_data = {
   PLUGIN_FLAG,                /* Plugin flag*/
   0xFFFFFFFF                  /* empty - extra data word */
 };
-#endif
 
-#if defined(__CC_ARM) || defined(__ARMCC_VERSION) || defined(__GNUC__)
 __attribute__((section(".boot_hdr.conf")))
-#elif defined(__ICCARM__)
-#pragma location = ".boot_hdr.conf"
-#endif
-
 // Values copied from https://github.com/PaulStoffregen/cores/blob/ddb23fa5d97dac763bc06e11b9b41f026bd51f0a/teensy4/bootdata.c#L39
 const flexspi_nor_config_t qspiflash_config = {
     .memConfig =
diff --git a/ports/mimxrt10xx/linking/common.ld b/ports/mimxrt10xx/linking/common.ld
index 5cead9ed0..10568bfb8 100644
--- a/ports/mimxrt10xx/linking/common.ld
+++ b/ports/mimxrt10xx/linking/common.ld
@@ -30,7 +30,9 @@ MEMORY
     ITCM (x)              : ORIGIN = 0x00000000, LENGTH = 32K
 }
 
-__RAM_VECTOR_TABLE_SIZE_BYTES = 0;
+__data_start__ = 0;
+__data_end__ = 0;
+_start = 0;
 
 SECTIONS
 {
@@ -81,31 +83,28 @@ SECTIONS
     .data :
     {
         . = ALIGN(4);
-        _sidata = .;
-        __data_start__ = .;     /* create a global symbol at data start */
-
         *(.data*)               /* .data* sections */
         *flexspi_nor_flash_ops.o(.text*)
         *fsl_flexspi.o(.text*)
         . = ALIGN(4);
-
-        __data_end__ = .;       /* define a global symbol at data end */
     } > OCRAM AT> FLASH_TEXT
+    _ld_ocram_data_destination = ADDR(.data);
+    _ld_ocram_data_flash_copy = LOADADDR(.data);
+    _ld_ocram_data_size = SIZEOF(.data);
 
     /* Uninitialized data section */
     .bss :
     {
         . = ALIGN(4);
-        __bss_start__ = .;
 
         *(.bss*)
         *(COMMON)
 
         . = ALIGN(4);
-        __bss_end__ = .;
-        _ld_heap_start = .;
-        PROVIDE(end = .);
     } > OCRAM
+    _ld_ocram_bss_start = ADDR(.bss);
+    _ld_ocram_bss_size = SIZEOF(.bss);
+    _ld_heap_start = _ld_ocram_bss_start + _ld_ocram_bss_size;
     _ld_heap_end = ORIGIN(OCRAM) + LENGTH(OCRAM);
 
     .itcm :
@@ -141,7 +140,7 @@ SECTIONS
         . = ALIGN(4);
     } > DTCM AT> DTCM
     _ld_dtcm_bss_start = ADDR(.dtcm_bss);
-    _ld_dtcm_bss_end = ADDR(.dtcm_bss) + SIZEOF(.dtcm_bss);
+    _ld_dtcm_bss_size = SIZEOF(.dtcm_bss);
 
     .stack :
     {
@@ -150,9 +149,6 @@ SECTIONS
         . += _ld_default_stack_size;
     } > DTCM
     _ld_stack_top = ORIGIN(DTCM) + LENGTH(DTCM);
-    _estack = _ld_stack_top;
 
     .ARM.attributes 0 : { *(.ARM.attributes) }
 }
-
-ASSERT(__data_end__ <= ORIGIN(FLASH_TEXT) + LENGTH(FLASH_TEXT), "region FLASH_TEXT overflowed with text and data")
diff --git a/ports/mimxrt10xx/peripherals/mimxrt10xx/MIMXRT1011/clocks.c b/ports/mimxrt10xx/peripherals/mimxrt10xx/MIMXRT1011/clocks.c
index c03991785..854ae4dee 100644
--- a/ports/mimxrt10xx/peripherals/mimxrt10xx/MIMXRT1011/clocks.c
+++ b/ports/mimxrt10xx/peripherals/mimxrt10xx/MIMXRT1011/clocks.c
@@ -129,19 +129,6 @@ void clocks_init(void) {
     CLOCK_DisableClock(kCLOCK_Pit);
     /* Set PERCLK_PODF. */
     CLOCK_SetDiv(kCLOCK_PerclkDiv, 1);
-    /* In SDK projects, external flash (configured by FLEXSPI) will be initialized by dcd.
-     * With this macro XIP_EXTERNAL_FLASH, usb1 pll (selected to be FLEXSPI clock source in SDK projects) will be left
-     * unchanged. Note: If another clock source is selected for FLEXSPI, user may want to avoid changing that clock as
-     * well.*/
-#if !(defined(XIP_EXTERNAL_FLASH) && (XIP_EXTERNAL_FLASH == 1))
-    /* Disable Flexspi clock gate. */
-    CLOCK_DisableClock(kCLOCK_FlexSpi);
-    /* Set FLEXSPI_PODF. */
-    CLOCK_SetDiv(kCLOCK_FlexspiDiv, 3);
-    /* Set Flexspi clock source. */
-    CLOCK_SetMux(kCLOCK_FlexspiMux, 0);
-    CLOCK_SetMux(kCLOCK_FlexspiSrcMux, 0);
-#endif
     /* Disable ADC_ACLK_EN clock gate. */
     CCM->CSCMR2 &= ~CCM_CSCMR2_ADC_ACLK_EN_MASK;
     /* Set ADC_ACLK_PODF. */
@@ -219,24 +206,6 @@ void clocks_init(void) {
     CLOCK_InitSysPfd(kCLOCK_Pfd2, 18);
     /* Init System pfd3. */
     CLOCK_InitSysPfd(kCLOCK_Pfd3, 18);
-    /* In SDK projects, external flash (configured by FLEXSPI) will be initialized by dcd.
-     * With this macro XIP_EXTERNAL_FLASH, usb1 pll (selected to be FLEXSPI clock source in SDK projects) will be left
-     * unchanged. Note: If another clock source is selected for FLEXSPI, user may want to avoid changing that clock as
-     * well.*/
-#if !(defined(XIP_EXTERNAL_FLASH) && (XIP_EXTERNAL_FLASH == 1))
-    /* Init Usb1 PLL. */
-    CLOCK_InitUsb1Pll(&usb1PllConfig_BOARD_BootClockRUN);
-    /* Init Usb1 pfd0. */
-    CLOCK_InitUsb1Pfd(kCLOCK_Pfd0, 22);
-    /* Init Usb1 pfd1. */
-    CLOCK_InitUsb1Pfd(kCLOCK_Pfd1, 16);
-    /* Init Usb1 pfd2. */
-    CLOCK_InitUsb1Pfd(kCLOCK_Pfd2, 17);
-    /* Init Usb1 pfd3. */
-    CLOCK_InitUsb1Pfd(kCLOCK_Pfd3, 18);
-    /* Disable Usb1 PLL output for USBPHY1. */
-    CCM_ANALOG->PLL_USB1 &= ~CCM_ANALOG_PLL_USB1_EN_USB_CLKS_MASK;
-#endif
     /* DeInit Audio PLL. */
     CLOCK_DeinitAudioPll();
     /* Bypass Audio PLL. */
diff --git a/ports/mimxrt10xx/peripherals/mimxrt10xx/MIMXRT1021/clocks.c b/ports/mimxrt10xx/peripherals/mimxrt10xx/MIMXRT1021/clocks.c
index 7dc08f6dd..cb9dd34c9 100644
--- a/ports/mimxrt10xx/peripherals/mimxrt10xx/MIMXRT1021/clocks.c
+++ b/ports/mimxrt10xx/peripherals/mimxrt10xx/MIMXRT1021/clocks.c
@@ -177,18 +177,6 @@ void clocks_init(void) {
     /* Set Semc clock source. */
     CLOCK_SetMux(kCLOCK_SemcMux, 0);
 #endif
-    /* In SDK projects, external flash (configured by FLEXSPI) will be initialized by dcd.
-     * With this macro XIP_EXTERNAL_FLASH, usb1 pll (selected to be FLEXSPI clock source in SDK projects) will be left
-     * unchanged. Note: If another clock source is selected for FLEXSPI, user may want to avoid changing that clock as
-     * well.*/
-#if !(defined(XIP_EXTERNAL_FLASH) && (XIP_EXTERNAL_FLASH == 1))
-    /* Disable Flexspi clock gate. */
-    CLOCK_DisableClock(kCLOCK_FlexSpi);
-    /* Set FLEXSPI_PODF. */
-    CLOCK_SetDiv(kCLOCK_FlexspiDiv, 3);
-    /* Set Flexspi clock source. */
-    CLOCK_SetMux(kCLOCK_FlexspiMux, 2);
-#endif
     /* Disable LPSPI clock gate. */
     CLOCK_DisableClock(kCLOCK_Lpspi1);
     CLOCK_DisableClock(kCLOCK_Lpspi2);
@@ -295,24 +283,6 @@ void clocks_init(void) {
     /* Init System pfd3. */
     CLOCK_InitSysPfd(kCLOCK_Pfd3, 18);
 #endif
-    /* In SDK projects, external flash (configured by FLEXSPI) will be initialized by dcd.
-     * With this macro XIP_EXTERNAL_FLASH, usb1 pll (selected to be FLEXSPI clock source in SDK projects) will be left
-     * unchanged. Note: If another clock source is selected for FLEXSPI, user may want to avoid changing that clock as
-     * well.*/
-#if !(defined(XIP_EXTERNAL_FLASH) && (XIP_EXTERNAL_FLASH == 1))
-    /* Init Usb1 PLL. */
-    CLOCK_InitUsb1Pll(&usb1PllConfig_BOARD_BootClockRUN);
-    /* Init Usb1 pfd0. */
-    CLOCK_InitUsb1Pfd(kCLOCK_Pfd0, 22);
-    /* Init Usb1 pfd1. */
-    CLOCK_InitUsb1Pfd(kCLOCK_Pfd1, 16);
-    /* Init Usb1 pfd2. */
-    CLOCK_InitUsb1Pfd(kCLOCK_Pfd2, 17);
-    /* Init Usb1 pfd3. */
-    CLOCK_InitUsb1Pfd(kCLOCK_Pfd3, 18);
-    /* Disable Usb1 PLL output for USBPHY1. */
-    CCM_ANALOG->PLL_USB1 &= ~CCM_ANALOG_PLL_USB1_EN_USB_CLKS_MASK;
-#endif
     /* DeInit Audio PLL. */
     CLOCK_DeinitAudioPll();
     /* Bypass Audio PLL. */
diff --git a/ports/mimxrt10xx/peripherals/mimxrt10xx/MIMXRT1062/clocks.c b/ports/mimxrt10xx/peripherals/mimxrt10xx/MIMXRT1062/clocks.c
index cafc8efc3..7b9af3a6a 100644
--- a/ports/mimxrt10xx/peripherals/mimxrt10xx/MIMXRT1062/clocks.c
+++ b/ports/mimxrt10xx/peripherals/mimxrt10xx/MIMXRT1062/clocks.c
@@ -130,17 +130,6 @@ void clocks_init(void) {
     /* Set Semc clock source. */
     CLOCK_SetMux(kCLOCK_SemcMux, 0);
 #endif
-    /* In SDK projects, external flash (configured by FLEXSPI) will be initialized by dcd.
-     * With this macro XIP_EXTERNAL_FLASH, usb1 pll (selected to be FLEXSPI clock source in SDK projects) will be left unchanged.
-     * Note: If another clock source is selected for FLEXSPI, user may want to avoid changing that clock as well.*/
-#if !(defined(XIP_EXTERNAL_FLASH) && (XIP_EXTERNAL_FLASH == 1))
-    /* Disable Flexspi clock gate. */
-    CLOCK_DisableClock(kCLOCK_FlexSpi);
-    /* Set FLEXSPI_PODF. */
-    CLOCK_SetDiv(kCLOCK_FlexspiDiv, 1);
-    /* Set Flexspi clock source. */
-    CLOCK_SetMux(kCLOCK_FlexspiMux, 3);
-#endif
     /* Disable LPSPI clock gate. */
     CLOCK_DisableClock(kCLOCK_Lpspi1);
     CLOCK_DisableClock(kCLOCK_Lpspi2);
@@ -255,23 +244,6 @@ void clocks_init(void) {
     /* Init System pfd3. */
     CLOCK_InitSysPfd(kCLOCK_Pfd3, 16);
 #endif
-    /* In SDK projects, external flash (configured by FLEXSPI) will be initialized by dcd.
-     * With this macro XIP_EXTERNAL_FLASH, usb1 pll (selected to be FLEXSPI clock source in SDK projects) will be left unchanged.
-     * Note: If another clock source is selected for FLEXSPI, user may want to avoid changing that clock as well.*/
-#if !(defined(XIP_EXTERNAL_FLASH) && (XIP_EXTERNAL_FLASH == 1))
-    /* Init Usb1 PLL. */
-    CLOCK_InitUsb1Pll(&usb1PllConfig_BOARD_BootClockRUN);
-    /* Init Usb1 pfd0. */
-    CLOCK_InitUsb1Pfd(kCLOCK_Pfd0, 33);
-    /* Init Usb1 pfd1. */
-    CLOCK_InitUsb1Pfd(kCLOCK_Pfd1, 16);
-    /* Init Usb1 pfd2. */
-    CLOCK_InitUsb1Pfd(kCLOCK_Pfd2, 17);
-    /* Init Usb1 pfd3. */
-    CLOCK_InitUsb1Pfd(kCLOCK_Pfd3, 19);
-    /* Disable Usb1 PLL output for USBPHY1. */
-    CCM_ANALOG->PLL_USB1 &= ~CCM_ANALOG_PLL_USB1_EN_USB_CLKS_MASK;
-#endif
     /* DeInit Audio PLL. */
     CLOCK_DeinitAudioPll();
     /* Bypass Audio PLL. */
diff --git a/ports/mimxrt10xx/supervisor/flexspi_nor_flash_ops.c b/ports/mimxrt10xx/supervisor/flexspi_nor_flash_ops.c
index e20c67fa3..71252d677 100644
--- a/ports/mimxrt10xx/supervisor/flexspi_nor_flash_ops.c
+++ b/ports/mimxrt10xx/supervisor/flexspi_nor_flash_ops.c
@@ -16,19 +16,10 @@
 
 static inline void flexspi_clock_init(void)
 {
-#if defined(XIP_EXTERNAL_FLASH) && (XIP_EXTERNAL_FLASH == 1)
     /* Switch to PLL2 for XIP to avoid hardfault during re-initialize clock. */
     CLOCK_InitSysPfd(kCLOCK_Pfd2, 24);    /* Set PLL2 PFD2 clock 396MHZ. */
     CLOCK_SetMux(kCLOCK_FlexspiMux, 0x2); /* Choose PLL2 PFD2 clock as flexspi source clock. */
     CLOCK_SetDiv(kCLOCK_FlexspiDiv, 2);   /* flexspi clock 133M. */
-#else
-    const clock_usb_pll_config_t g_ccmConfigUsbPll = {.loopDivider = 0U};
-
-    CLOCK_InitUsb1Pll(&g_ccmConfigUsbPll);
-    CLOCK_InitUsb1Pfd(kCLOCK_Pfd0, 24);   /* Set PLL3 PFD0 clock 360MHZ. */
-    CLOCK_SetMux(kCLOCK_FlexspiMux, 0x3); /* Choose PLL3 PFD0 clock as flexspi source clock. */
-    CLOCK_SetDiv(kCLOCK_FlexspiDiv, 2);   /* flexspi clock 120M. */
-#endif
 }
 
 extern flexspi_device_config_t deviceconfig;
diff --git a/ports/mimxrt10xx/supervisor/port.c b/ports/mimxrt10xx/supervisor/port.c
index e141bc6b8..2c5e838e2 100644
--- a/ports/mimxrt10xx/supervisor/port.c
+++ b/ports/mimxrt10xx/supervisor/port.c
@@ -83,28 +83,38 @@
 #define NO_SUBREGIONS 0
 
 extern uint32_t _ld_flash_size;
+extern uint32_t _ld_stack_top;
+
+extern uint32_t __isr_vector[];
+
+extern uint32_t _ld_ocram_bss_start;
+extern uint32_t _ld_ocram_bss_size;
+extern uint32_t _ld_ocram_data_destination;
+extern uint32_t _ld_ocram_data_size;
+extern uint32_t _ld_ocram_data_flash_copy;
+extern uint32_t _ld_dtcm_bss_start;
+extern uint32_t _ld_dtcm_bss_size;
+extern uint32_t _ld_dtcm_data_destination;
+extern uint32_t _ld_dtcm_data_size;
+extern uint32_t _ld_dtcm_data_flash_copy;
+extern uint32_t _ld_itcm_destination;
+extern uint32_t _ld_itcm_size;
+extern uint32_t _ld_itcm_flash_copy;
+
+extern void main(void);
+
+// This replaces the Reset_Handler in startup_*.S and SystemInit in system_*.c.
+__attribute__((used, naked)) void Reset_Handler(void) {
+    __disable_irq();
+    SCB->VTOR = (uint32_t) &__isr_vector;
+    __set_MSP((uint32_t) &_ld_stack_top);
 
-extern uint8_t _ld_dtcm_bss_start;
-extern uint8_t _ld_dtcm_bss_end;
-extern uint8_t _ld_dtcm_data_destination;
-extern uint8_t _ld_dtcm_data_size;
-extern uint8_t _ld_dtcm_data_flash_copy;
-extern uint8_t _ld_itcm_destination;
-extern uint8_t _ld_itcm_size;
-extern uint8_t _ld_itcm_flash_copy;
-
-// This is called before RAM is setup! Be very careful what you do here.
-void SystemInitHook(void) {
-    // asm("bkpt");
     /* Disable I cache and D cache */
-    if (SCB_CCR_IC_Msk == (SCB_CCR_IC_Msk & SCB->CCR))
-    {
-        SCB_DisableICache();
-    }
-    if (SCB_CCR_DC_Msk == (SCB_CCR_DC_Msk & SCB->CCR))
-    {
-        SCB_DisableDCache();
-    }
+    SCB_DisableICache();
+    SCB_DisableDCache();
+
+    // Changing the FlexRAM must happen here where the stack is empty. If it is in a function call,
+    // then the return will jump to an invalid address.
     // Configure FlexRAM. The e is one block of ITCM (0b11) and DTCM (0b10). The rest is two OCRAM
     // (0b01). We shift in zeroes for all unimplemented banks.
     IOMUXC_GPR->GPR17 = (0xe5555555) >> (32 - 2 * FSL_FEATURE_FLEXRAM_INTERNAL_RAM_TOTAL_BANK_NUMBERS);
@@ -120,17 +130,35 @@ void SystemInitHook(void) {
     current_gpr14 |= IOMUXC_GPR_GPR14_CM7_CFGITCMSZ(0x6);
     IOMUXC_GPR->GPR14 = current_gpr14;
 
-    /* Disable MPU */
-    ARM_MPU_Disable();
+    #if ((__FPU_PRESENT == 1) && (__FPU_USED == 1))
+      SCB->CPACR |= ((3UL << 10*2) | (3UL << 11*2));    /* set CP10, CP11 Full Access */
+    #endif /* ((__FPU_PRESENT == 1) && (__FPU_USED == 1)) */
 
-    // Copy all of the code to run from ITCM.
-    memcpy(&_ld_itcm_destination, &_ld_itcm_flash_copy, (size_t) &_ld_itcm_size);
+    /* Disable Watchdog Power Down Counter */
+    WDOG1->WMCR &= ~WDOG_WMCR_PDE_MASK;
+    WDOG2->WMCR &= ~WDOG_WMCR_PDE_MASK;
 
-    // Copy all of the data to run from DTCM.
-    memcpy(&_ld_dtcm_data_destination, &_ld_dtcm_data_flash_copy, (size_t) &_ld_dtcm_data_size);
+    /* Watchdog disable */
+    WDOG1->WCR &= ~WDOG_WCR_WDE_MASK;
+    WDOG2->WCR &= ~WDOG_WCR_WDE_MASK;
+    RTWDOG->CNT = 0xD928C520U; /* 0xD928C520U is the update key */
+    RTWDOG->TOVAL = 0xFFFF;
+    RTWDOG->CS = (uint32_t) ((RTWDOG->CS) & ~RTWDOG_CS_EN_MASK) | RTWDOG_CS_UPDATE_MASK;
 
-    // Clear DTCM bss.
-    memset(&_ld_dtcm_bss_start, 0, (size_t) (&_ld_dtcm_bss_end - &_ld_dtcm_bss_start));
+    /* Disable Systick which might be enabled by bootrom */
+    if (SysTick->CTRL & SysTick_CTRL_ENABLE_Msk)
+    {
+        SysTick->CTRL &= ~SysTick_CTRL_ENABLE_Msk;
+    }
+
+    /* Disable MPU */
+    ARM_MPU_Disable();
+
+    // Copy all of the code to run from ITCM. Do this while the MPU is disabled because we write
+    // protect it.
+    for (uint32_t i = 0; i < ((size_t) &_ld_itcm_size) / 4; i++) {
+        (&_ld_itcm_destination)[i] = (&_ld_itcm_flash_copy)[i];
+    }
 
     // The first number in RBAR is the region number. When searching for a policy, the region with
     // the highest number wins. If none match, then the default policy set at enable applies.
@@ -187,9 +215,32 @@ void SystemInitHook(void) {
     /* Enable MPU */
     ARM_MPU_Enable(MPU_CTRL_PRIVDEFENA_Msk);
 
-    /* Enable I cache and D cache */
+    /* We're done mucking with memory so enable I cache and D cache */
     SCB_EnableDCache();
     SCB_EnableICache();
+
+    // Copy all of the data to run from DTCM.
+    for (uint32_t i = 0; i < ((size_t) &_ld_dtcm_data_size) / 4; i++) {
+        (&_ld_dtcm_data_destination)[i] = (&_ld_dtcm_data_flash_copy)[i];
+    }
+
+    // Clear DTCM bss.
+    for (uint32_t i = 0; i < ((size_t) &_ld_dtcm_bss_size) / 4; i++) {
+        (&_ld_dtcm_bss_start)[i] = 0;
+    }
+
+    // Copy all of the data to run from OCRAM.
+    for (uint32_t i = 0; i < ((size_t) &_ld_ocram_data_size) / 4; i++) {
+        (&_ld_ocram_data_destination)[i] = (&_ld_ocram_data_flash_copy)[i];
+    }
+
+    // Clear OCRAM bss.
+    for (uint32_t i = 0; i < ((size_t) &_ld_ocram_bss_size) / 4; i++) {
+        (&_ld_ocram_bss_start)[i] = 0;
+    }
+
+    __enable_irq();
+    main();
 }
 
 safe_mode_t port_init(void) {