diff --git a/teensy4/avr/pgmspace.h b/teensy4/avr/pgmspace.h index 81f2bc03f..00e3b0ed0 100644 --- a/teensy4/avr/pgmspace.h +++ b/teensy4/avr/pgmspace.h @@ -31,6 +31,10 @@ #define FLASHMEM __attribute__((section(".flashmem"))) #define EXTMEM __attribute__((section(".externalram"))) +#define REGISTER_EARLY_STARTUP(f) static void (* f ## _eh_ptr)(void) __attribute__ ((section(".early_hook"),used)) = f +#define REGISTER_MIDDLE_STARTUP(f) static void (* f ## _mh_ptr)(void) __attribute__ ((section(".middle_hook"),used)) = f +#define REGISTER_LATE_STARTUP(f) static void (* f ## _lh_ptr)(void) __attribute__ ((section(".late_hook"),used)) = f + #define PGM_P const char * #define PSTR(str) ({static const char data[] PROGMEM = (str); &data[0];}) diff --git a/teensy4/imxrt1062.ld b/teensy4/imxrt1062.ld index 4cad74511..18bd213d5 100644 --- a/teensy4/imxrt1062.ld +++ b/teensy4/imxrt1062.ld @@ -23,6 +23,15 @@ SECTIONS KEEP(*(.startup)) *(.flashmem*) . = ALIGN(4); + _startup_early_array = .; + KEEP(*(.early_hook)) + LONG(0); + _startup_middle_array = .; + KEEP(*(.middle_hook)) + LONG(0); + _startup_late_array = .; + KEEP(*(.late_hook)) + LONG(0); KEEP(*(.init)) __preinit_array_start = .; KEEP (*(.preinit_array)) diff --git a/teensy4/imxrt1062_mm.ld b/teensy4/imxrt1062_mm.ld index 37d59ceb8..9688a7689 100644 --- a/teensy4/imxrt1062_mm.ld +++ b/teensy4/imxrt1062_mm.ld @@ -23,6 +23,15 @@ SECTIONS KEEP(*(.startup)) *(.flashmem*) . = ALIGN(4); + _startup_early_array = .; + KEEP(*(.early_hook)) + LONG(0); + _startup_middle_array = .; + KEEP(*(.middle_hook)) + LONG(0); + _startup_late_array = .; + KEEP(*(.late_hook)) + LONG(0); KEEP(*(.init)) __preinit_array_start = .; KEEP (*(.preinit_array)) diff --git a/teensy4/imxrt1062_t41.ld b/teensy4/imxrt1062_t41.ld index 6220e8cc7..5ba3c6ccc 100644 --- a/teensy4/imxrt1062_t41.ld +++ b/teensy4/imxrt1062_t41.ld @@ -24,6 +24,15 @@ SECTIONS KEEP(*(.startup)) *(.flashmem*) . = ALIGN(4); + _startup_early_array = .; + KEEP(*(.early_hook)) + LONG(0); + _startup_middle_array = .; + KEEP(*(.middle_hook)) + LONG(0); + _startup_late_array = .; + KEEP(*(.late_hook)) + LONG(0); KEEP(*(.init)) __preinit_array_start = .; KEEP (*(.preinit_array)) diff --git a/teensy4/startup.c b/teensy4/startup.c index 2375f26c2..c04896721 100644 --- a/teensy4/startup.c +++ b/teensy4/startup.c @@ -20,6 +20,9 @@ extern unsigned long _flexram_bank_config; extern unsigned long _estack; extern unsigned long _extram_start; extern unsigned long _extram_end; +extern void (*_startup_early_array)(void); +extern void (*_startup_middle_array)(void); +extern void (*_startup_late_array)(void); __attribute__ ((used, aligned(1024), section(".vectorsram"))) void (* volatile _VectorsRam[NVIC_NUM_INTERRUPTS+16])(void); @@ -48,15 +51,46 @@ struct smalloc_pool extmem_smalloc_pool; #endif extern int main (void); -FLASHMEM void startup_default_early_hook(void) {} -void startup_early_hook(void) __attribute__ ((weak, alias("startup_default_early_hook"))); -FLASHMEM void startup_default_middle_hook(void) {} -void startup_middle_hook(void) __attribute__ ((weak, alias("startup_default_middle_hook"))); -FLASHMEM void startup_default_late_hook(void) {} -void startup_late_hook(void) __attribute__ ((weak, alias("startup_default_late_hook"))); +FLASHMEM void startup_default_hook(void) {} +void startup_early_hook(void) __attribute__ ((weak, alias("startup_default_hook"))); +void startup_middle_hook(void) __attribute__ ((weak, alias("startup_default_hook"))); +void startup_late_hook(void) __attribute__ ((weak, alias("startup_default_hook"))); extern void startup_debug_reset(void) __attribute__((noinline)); FLASHMEM void startup_debug_reset(void) { __asm__ volatile("nop"); } +FLASHMEM static void run_startup_early_hooks(void) +{ + startup_early_hook(); + void (**f)(void) = &_startup_early_array; + while ((*f) != NULL) + { + (*f)(); + f++; + } +} + +FLASHMEM static void run_startup_middle_hooks(void) +{ + startup_middle_hook(); + void (**f)(void) = &_startup_middle_array; + while ((*f) != NULL) + { + (*f)(); + f++; + } +} + +FLASHMEM static void run_startup_late_hooks(void) +{ + startup_late_hook(); + void (**f)(void) = &_startup_late_array; + while ((*f) != NULL) + { + (*f)(); + f++; + } +} + static void ResetHandler2(void); __attribute__((section(".startup"), naked)) @@ -86,7 +120,7 @@ static void ResetHandler2(void) asm volatile("nop"); asm volatile("nop"); #endif - startup_early_hook(); // must be in FLASHMEM, as ITCM is not yet initialized! + run_startup_early_hooks(); // must be in FLASHMEM, as ITCM is not yet initialized! PMU_MISC0_SET = 1<<3; //Use bandgap-based bias currents for best performance (Page 1175) #if 1 // Some optimization with LTO won't start without this delay, but why? @@ -183,7 +217,7 @@ static void ResetHandler2(void) analog_init(); pwm_init(); tempmon_init(); - startup_middle_hook(); + run_startup_middle_hooks(); #if !defined(TEENSY_INIT_USB_DELAY_BEFORE) #define TEENSY_INIT_USB_DELAY_BEFORE 20 @@ -200,7 +234,7 @@ static void ResetHandler2(void) while (millis() < TEENSY_INIT_USB_DELAY_AFTER + TEENSY_INIT_USB_DELAY_BEFORE) ; // wait //printf("before C++ constructors\n"); startup_debug_reset(); - startup_late_hook(); + run_startup_late_hooks(); __libc_init_array(); //printf("after C++ constructors\n"); //printf("before setup\n");