Lib/mk20dx.c

   1 /* Teensyduino Core Library
   2  * http://www.pjrc.com/teensy/
   3  * Copyright (c) 2013 PJRC.COM, LLC.
   4  * Modifications by Jacob Alexander 2014
   5  *
   6  * Permission is hereby granted, free of charge, to any person obtaining
   7  * a copy of this software and associated documentation files (the
   8  * "Software"), to deal in the Software without restriction, including
   9  * without limitation the rights to use, copy, modify, merge, publish,
  10  * distribute, sublicense, and/or sell copies of the Software, and to
  11  * permit persons to whom the Software is furnished to do so, subject to
  12  * the following conditions:
  13  *
  14  * 1. The above copyright notice and this permission notice shall be
  15  * included in all copies or substantial portions of the Software.
  16  *
  17  * 2. If the Software is incorporated into a build system that allows
  18  * selection among a list of target devices, then similar target
  19  * devices manufactured by PJRC.COM must be included in the list of
  20  * target devices and selectable in the same manner.
  21  *
  22  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
  23  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
  24  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
  25  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
  26  * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
  27  * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
  28  * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  29  * SOFTWARE.
  30  */
  31
  32 // Local Includes
  33 #include "mk20dx.h"
  34
  35
  36
  37 // ----- Variables -----
  38
  39 extern unsigned long _stext;
  40 extern unsigned long _etext;
  41 extern unsigned long _sdata;
  42 extern unsigned long _edata;
  43 extern unsigned long _sbss;
  44 extern unsigned long _ebss;
  45 extern unsigned long _estack;
  46
  47 const uint8_t sys_reset_to_loader_magic[22] = "\xff\x00\x7fRESET TO LOADER\x7f\x00\xff";
  48
  49
  50
  51 // ----- Function Declarations -----
  52
  53 extern int main();
  54 void ResetHandler();
  55
  56
  57
  58 // ----- Interrupts -----
  59
  60 // NVIC - Default ISR
  61 void fault_isr()
  62 {
  63         while ( 1 )
  64         {
  65                 // keep polling some communication while in fault
  66                 // mode, so we don't completely die.
  67                 if ( SIM_SCGC4 & SIM_SCGC4_USBOTG ) usb_isr();
  68                 if ( SIM_SCGC4 & SIM_SCGC4_UART0 )  uart0_status_isr();
  69                 if ( SIM_SCGC4 & SIM_SCGC4_UART1 )  uart1_status_isr();
  70                 if ( SIM_SCGC4 & SIM_SCGC4_UART2 )  uart2_status_isr();
  71         }
  72 }
  73
  74 void unused_isr()
  75 {
  76         fault_isr();
  77 }
  78
  79
  80 // NVIC - SysTick ISR
  81 extern volatile uint32_t systick_millis_count;
  82 void systick_default_isr()
  83 {
  84         systick_millis_count++;
  85 }
  86
  87
  88 // NVIC - Default ISR/Vector Linking
  89 void nmi_isr()              __attribute__ ((weak, alias("unused_isr")));
  90 void hard_fault_isr()       __attribute__ ((weak, alias("unused_isr")));
  91 void memmanage_fault_isr()  __attribute__ ((weak, alias("unused_isr")));
  92 void bus_fault_isr()        __attribute__ ((weak, alias("unused_isr")));
  93 void usage_fault_isr()      __attribute__ ((weak, alias("unused_isr")));
  94 void svcall_isr()           __attribute__ ((weak, alias("unused_isr")));
  95 void debugmonitor_isr()     __attribute__ ((weak, alias("unused_isr")));
  96 void pendablesrvreq_isr()   __attribute__ ((weak, alias("unused_isr")));
  97 void systick_isr()          __attribute__ ((weak, alias("systick_default_isr")));
  98
  99 void dma_ch0_isr()          __attribute__ ((weak, alias("unused_isr")));
 100 void dma_ch1_isr()          __attribute__ ((weak, alias("unused_isr")));
 101 void dma_ch2_isr()          __attribute__ ((weak, alias("unused_isr")));
 102 void dma_ch3_isr()          __attribute__ ((weak, alias("unused_isr")));
 103 void dma_ch4_isr()          __attribute__ ((weak, alias("unused_isr")));
 104 void dma_ch5_isr()          __attribute__ ((weak, alias("unused_isr")));
 105 void dma_ch6_isr()          __attribute__ ((weak, alias("unused_isr")));
 106 void dma_ch7_isr()          __attribute__ ((weak, alias("unused_isr")));
 107 void dma_ch8_isr()          __attribute__ ((weak, alias("unused_isr")));
 108 void dma_ch9_isr()          __attribute__ ((weak, alias("unused_isr")));
 109 void dma_ch10_isr()         __attribute__ ((weak, alias("unused_isr")));
 110 void dma_ch11_isr()         __attribute__ ((weak, alias("unused_isr")));
 111 void dma_ch12_isr()         __attribute__ ((weak, alias("unused_isr")));
 112 void dma_ch13_isr()         __attribute__ ((weak, alias("unused_isr")));
 113 void dma_ch14_isr()         __attribute__ ((weak, alias("unused_isr")));
 114 void dma_ch15_isr()         __attribute__ ((weak, alias("unused_isr")));
 115 void dma_error_isr()        __attribute__ ((weak, alias("unused_isr")));
 116 void mcm_isr()              __attribute__ ((weak, alias("unused_isr")));
 117 void flash_cmd_isr()        __attribute__ ((weak, alias("unused_isr")));
 118 void flash_error_isr()      __attribute__ ((weak, alias("unused_isr")));
 119 void low_voltage_isr()      __attribute__ ((weak, alias("unused_isr")));
 120 void wakeup_isr()           __attribute__ ((weak, alias("unused_isr")));
 121 void watchdog_isr()         __attribute__ ((weak, alias("unused_isr")));
 122 void i2c0_isr()             __attribute__ ((weak, alias("unused_isr")));
 123 void i2c1_isr()             __attribute__ ((weak, alias("unused_isr")));
 124 void i2c2_isr()             __attribute__ ((weak, alias("unused_isr")));
 125 void spi0_isr()             __attribute__ ((weak, alias("unused_isr")));
 126 void spi1_isr()             __attribute__ ((weak, alias("unused_isr")));
 127 void spi2_isr()             __attribute__ ((weak, alias("unused_isr")));
 128 void sdhc_isr()             __attribute__ ((weak, alias("unused_isr")));
 129 void can0_message_isr()     __attribute__ ((weak, alias("unused_isr")));
 130 void can0_bus_off_isr()     __attribute__ ((weak, alias("unused_isr")));
 131 void can0_error_isr()       __attribute__ ((weak, alias("unused_isr")));
 132 void can0_tx_warn_isr()     __attribute__ ((weak, alias("unused_isr")));
 133 void can0_rx_warn_isr()     __attribute__ ((weak, alias("unused_isr")));
 134 void can0_wakeup_isr()      __attribute__ ((weak, alias("unused_isr")));
 135 void i2s0_tx_isr()          __attribute__ ((weak, alias("unused_isr")));
 136 void i2s0_rx_isr()          __attribute__ ((weak, alias("unused_isr")));
 137 void uart0_lon_isr()        __attribute__ ((weak, alias("unused_isr")));
 138 void uart0_status_isr()     __attribute__ ((weak, alias("unused_isr")));
 139 void uart0_error_isr()      __attribute__ ((weak, alias("unused_isr")));
 140 void uart1_status_isr()     __attribute__ ((weak, alias("unused_isr")));
 141 void uart1_error_isr()      __attribute__ ((weak, alias("unused_isr")));
 142 void uart2_status_isr()     __attribute__ ((weak, alias("unused_isr")));
 143 void uart2_error_isr()      __attribute__ ((weak, alias("unused_isr")));
 144 void uart3_status_isr()     __attribute__ ((weak, alias("unused_isr")));
 145 void uart3_error_isr()      __attribute__ ((weak, alias("unused_isr")));
 146 void uart4_status_isr()     __attribute__ ((weak, alias("unused_isr")));
 147 void uart4_error_isr()      __attribute__ ((weak, alias("unused_isr")));
 148 void uart5_status_isr()     __attribute__ ((weak, alias("unused_isr")));
 149 void uart5_error_isr()      __attribute__ ((weak, alias("unused_isr")));
 150 void adc0_isr()             __attribute__ ((weak, alias("unused_isr")));
 151 void adc1_isr()             __attribute__ ((weak, alias("unused_isr")));
 152 void cmp0_isr()             __attribute__ ((weak, alias("unused_isr")));
 153 void cmp1_isr()             __attribute__ ((weak, alias("unused_isr")));
 154 void cmp2_isr()             __attribute__ ((weak, alias("unused_isr")));
 155 void ftm0_isr()             __attribute__ ((weak, alias("unused_isr")));
 156 void ftm1_isr()             __attribute__ ((weak, alias("unused_isr")));
 157 void ftm2_isr()             __attribute__ ((weak, alias("unused_isr")));
 158 void ftm3_isr()             __attribute__ ((weak, alias("unused_isr")));
 159 void cmt_isr()              __attribute__ ((weak, alias("unused_isr")));
 160 void rtc_alarm_isr()        __attribute__ ((weak, alias("unused_isr")));
 161 void rtc_seconds_isr()      __attribute__ ((weak, alias("unused_isr")));
 162 void pit0_isr()             __attribute__ ((weak, alias("unused_isr")));
 163 void pit1_isr()             __attribute__ ((weak, alias("unused_isr")));
 164 void pit2_isr()             __attribute__ ((weak, alias("unused_isr")));
 165 void pit3_isr()             __attribute__ ((weak, alias("unused_isr")));
 166 void pdb_isr()              __attribute__ ((weak, alias("unused_isr")));
 167 void usb_isr()              __attribute__ ((weak, alias("unused_isr")));
 168 void usb_charge_isr()       __attribute__ ((weak, alias("unused_isr")));
 169 void dac0_isr()             __attribute__ ((weak, alias("unused_isr")));
 170 void dac1_isr()             __attribute__ ((weak, alias("unused_isr")));
 171 void tsi0_isr()             __attribute__ ((weak, alias("unused_isr")));
 172 void mcg_isr()              __attribute__ ((weak, alias("unused_isr")));
 173 void lptmr_isr()            __attribute__ ((weak, alias("unused_isr")));
 174 void porta_isr()            __attribute__ ((weak, alias("unused_isr")));
 175 void portb_isr()            __attribute__ ((weak, alias("unused_isr")));
 176 void portc_isr()            __attribute__ ((weak, alias("unused_isr")));
 177 void portd_isr()            __attribute__ ((weak, alias("unused_isr")));
 178 void porte_isr()            __attribute__ ((weak, alias("unused_isr")));
 179 void software_isr()         __attribute__ ((weak, alias("unused_isr")));
 180
 181
 182 // NVIC - Interrupt Vector Table
 183 __attribute__ ((section(".vectors"), used))
 184 void (* const gVectors[])() =
 185 {
 186         (void (*)(void))((unsigned long)&_estack),      //  0 ARM: Initial Stack Pointer
 187         ResetHandler,                                   //  1 ARM: Initial Program Counter
 188         nmi_isr,                                        //  2 ARM: Non-maskable Interrupt (NMI)
 189         hard_fault_isr,                                 //  3 ARM: Hard Fault
 190         memmanage_fault_isr,                            //  4 ARM: MemManage Fault
 191         bus_fault_isr,                                  //  5 ARM: Bus Fault
 192         usage_fault_isr,                                //  6 ARM: Usage Fault
 193         fault_isr,                                      //  7 --
 194         fault_isr,                                      //  8 --
 195         fault_isr,                                      //  9 --
 196         fault_isr,                                      // 10 --
 197         svcall_isr,                                     // 11 ARM: Supervisor call (SVCall)
 198         debugmonitor_isr,                               // 12 ARM: Debug Monitor
 199         fault_isr,                                      // 13 --
 200         pendablesrvreq_isr,                             // 14 ARM: Pendable req serv(PendableSrvReq)
 201         systick_isr,                                    // 15 ARM: System tick timer (SysTick)
 202 #if defined(_mk20dx128_) || defined(_mk20dx128vlf5_)
 203         dma_ch0_isr,                                    // 16 DMA channel 0 transfer complete
 204         dma_ch1_isr,                                    // 17 DMA channel 1 transfer complete
 205         dma_ch2_isr,                                    // 18 DMA channel 2 transfer complete
 206         dma_ch3_isr,                                    // 19 DMA channel 3 transfer complete
 207         dma_error_isr,                                  // 20 DMA error interrupt channel
 208         unused_isr,                                     // 21 DMA --
 209         flash_cmd_isr,                                  // 22 Flash Memory Command complete
 210         flash_error_isr,                                // 23 Flash Read collision
 211         low_voltage_isr,                                // 24 Low-voltage detect/warning
 212         wakeup_isr,                                     // 25 Low Leakage Wakeup
 213         watchdog_isr,                                   // 26 Both EWM and WDOG interrupt
 214         i2c0_isr,                                       // 27 I2C0
 215         spi0_isr,                                       // 28 SPI0
 216         i2s0_tx_isr,                                    // 29 I2S0 Transmit
 217         i2s0_rx_isr,                                    // 30 I2S0 Receive
 218         uart0_lon_isr,                                  // 31 UART0 CEA709.1-B (LON) status
 219         uart0_status_isr,                               // 32 UART0 status
 220         uart0_error_isr,                                // 33 UART0 error
 221         uart1_status_isr,                               // 34 UART1 status
 222         uart1_error_isr,                                // 35 UART1 error
 223         uart2_status_isr,                               // 36 UART2 status
 224         uart2_error_isr,                                // 37 UART2 error
 225         adc0_isr,                                       // 38 ADC0
 226         cmp0_isr,                                       // 39 CMP0
 227         cmp1_isr,                                       // 40 CMP1
 228         ftm0_isr,                                       // 41 FTM0
 229         ftm1_isr,                                       // 42 FTM1
 230         cmt_isr,                                        // 43 CMT
 231         rtc_alarm_isr,                                  // 44 RTC Alarm interrupt
 232         rtc_seconds_isr,                                // 45 RTC Seconds interrupt
 233         pit0_isr,                                       // 46 PIT Channel 0
 234         pit1_isr,                                       // 47 PIT Channel 1
 235         pit2_isr,                                       // 48 PIT Channel 2
 236         pit3_isr,                                       // 49 PIT Channel 3
 237         pdb_isr,                                        // 50 PDB Programmable Delay Block
 238         usb_isr,                                        // 51 USB OTG
 239         usb_charge_isr,                                 // 52 USB Charger Detect
 240         tsi0_isr,                                       // 53 TSI0
 241         mcg_isr,                                        // 54 MCG
 242         lptmr_isr,                                      // 55 Low Power Timer
 243         porta_isr,                                      // 56 Pin detect (Port A)
 244         portb_isr,                                      // 57 Pin detect (Port B)
 245         portc_isr,                                      // 58 Pin detect (Port C)
 246         portd_isr,                                      // 59 Pin detect (Port D)
 247         porte_isr,                                      // 60 Pin detect (Port E)
 248         software_isr,                                   // 61 Software interrupt
 249 #elif defined(_mk20dx256_)
 250         dma_ch0_isr,                                    // 16 DMA channel 0 transfer complete
 251         dma_ch1_isr,                                    // 17 DMA channel 1 transfer complete
 252         dma_ch2_isr,                                    // 18 DMA channel 2 transfer complete
 253         dma_ch3_isr,                                    // 19 DMA channel 3 transfer complete
 254         dma_ch4_isr,                                    // 20 DMA channel 4 transfer complete
 255         dma_ch5_isr,                                    // 21 DMA channel 5 transfer complete
 256         dma_ch6_isr,                                    // 22 DMA channel 6 transfer complete
 257         dma_ch7_isr,                                    // 23 DMA channel 7 transfer complete
 258         dma_ch8_isr,                                    // 24 DMA channel 8 transfer complete
 259         dma_ch9_isr,                                    // 25 DMA channel 9 transfer complete
 260         dma_ch10_isr,                                   // 26 DMA channel 10 transfer complete
 261         dma_ch11_isr,                                   // 27 DMA channel 10 transfer complete
 262         dma_ch12_isr,                                   // 28 DMA channel 10 transfer complete
 263         dma_ch13_isr,                                   // 29 DMA channel 10 transfer complete
 264         dma_ch14_isr,                                   // 30 DMA channel 10 transfer complete
 265         dma_ch15_isr,                                   // 31 DMA channel 10 transfer complete
 266         dma_error_isr,                                  // 32 DMA error interrupt channel
 267         unused_isr,                                     // 33 --
 268         flash_cmd_isr,                                  // 34 Flash Memory Command complete
 269         flash_error_isr,                                // 35 Flash Read collision
 270         low_voltage_isr,                                // 36 Low-voltage detect/warning
 271         wakeup_isr,                                     // 37 Low Leakage Wakeup
 272         watchdog_isr,                                   // 38 Both EWM and WDOG interrupt
 273         unused_isr,                                     // 39 --
 274         i2c0_isr,                                       // 40 I2C0
 275         i2c1_isr,                                       // 41 I2C1
 276         spi0_isr,                                       // 42 SPI0
 277         spi1_isr,                                       // 43 SPI1
 278         unused_isr,                                     // 44 --
 279         can0_message_isr,                               // 45 CAN OR'ed Message buffer (0-15)
 280         can0_bus_off_isr,                               // 46 CAN Bus Off
 281         can0_error_isr,                                 // 47 CAN Error
 282         can0_tx_warn_isr,                               // 48 CAN Transmit Warning
 283         can0_rx_warn_isr,                               // 49 CAN Receive Warning
 284         can0_wakeup_isr,                                // 50 CAN Wake Up
 285         i2s0_tx_isr,                                    // 51 I2S0 Transmit
 286         i2s0_rx_isr,                                    // 52 I2S0 Receive
 287         unused_isr,                                     // 53 --
 288         unused_isr,                                     // 54 --
 289         unused_isr,                                     // 55 --
 290         unused_isr,                                     // 56 --
 291         unused_isr,                                     // 57 --
 292         unused_isr,                                     // 58 --
 293         unused_isr,                                     // 59 --
 294         uart0_lon_isr,                                  // 60 UART0 CEA709.1-B (LON) status
 295         uart0_status_isr,                               // 61 UART0 status
 296         uart0_error_isr,                                // 62 UART0 error
 297         uart1_status_isr,                               // 63 UART1 status
 298         uart1_error_isr,                                // 64 UART1 error
 299         uart2_status_isr,                               // 65 UART2 status
 300         uart2_error_isr,                                // 66 UART2 error
 301         unused_isr,                                     // 67 --
 302         unused_isr,                                     // 68 --
 303         unused_isr,                                     // 69 --
 304         unused_isr,                                     // 70 --
 305         unused_isr,                                     // 71 --
 306         unused_isr,                                     // 72 --
 307         adc0_isr,                                       // 73 ADC0
 308         adc1_isr,                                       // 74 ADC1
 309         cmp0_isr,                                       // 75 CMP0
 310         cmp1_isr,                                       // 76 CMP1
 311         cmp2_isr,                                       // 77 CMP2
 312         ftm0_isr,                                       // 78 FTM0
 313         ftm1_isr,                                       // 79 FTM1
 314         ftm2_isr,                                       // 80 FTM2
 315         cmt_isr,                                        // 81 CMT
 316         rtc_alarm_isr,                                  // 82 RTC Alarm interrupt
 317         rtc_seconds_isr,                                // 83 RTC Seconds interrupt
 318         pit0_isr,                                       // 84 PIT Channel 0
 319         pit1_isr,                                       // 85 PIT Channel 1
 320         pit2_isr,                                       // 86 PIT Channel 2
 321         pit3_isr,                                       // 87 PIT Channel 3
 322         pdb_isr,                                        // 88 PDB Programmable Delay Block
 323         usb_isr,                                        // 89 USB OTG
 324         usb_charge_isr,                                 // 90 USB Charger Detect
 325         unused_isr,                                     // 91 --
 326         unused_isr,                                     // 92 --
 327         unused_isr,                                     // 93 --
 328         unused_isr,                                     // 94 --
 329         unused_isr,                                     // 95 --
 330         unused_isr,                                     // 96 --
 331         dac0_isr,                                       // 97 DAC0
 332         unused_isr,                                     // 98 --
 333         tsi0_isr,                                       // 99 TSI0
 334         mcg_isr,                                        // 100 MCG
 335         lptmr_isr,                                      // 101 Low Power Timer
 336         unused_isr,                                     // 102 --
 337         porta_isr,                                      // 103 Pin detect (Port A)
 338         portb_isr,                                      // 104 Pin detect (Port B)
 339         portc_isr,                                      // 105 Pin detect (Port C)
 340         portd_isr,                                      // 106 Pin detect (Port D)
 341         porte_isr,                                      // 107 Pin detect (Port E)
 342         unused_isr,                                     // 108 --
 343         unused_isr,                                     // 109 --
 344         software_isr,                                   // 110 Software interrupt
 345 #endif
 346 };
 347
 348
 349 // ----- Flash Configuration -----
 350
 351 // Only necessary for Teensy 3s, MCHCK uses the Bootloader to handle this
 352 #if defined(_mk20dx128_) || defined(_mk20dx256_)
 353 __attribute__ ((section(".flashconfig"), used))
 354 const uint8_t flashconfigbytes[16] = {
 355         0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
 356         0xFF, 0xFF, 0xFF, 0xFF, 0xFE, 0xFF, 0xFF, 0xFF
 357 };
 358 #elif defined(_mk20dx128vlf5_) && defined(_bootloader_)
 359 // XXX Byte labels may be in incorrect positions, double check before modifying
 360 //     FSEC is in correct location -Jacob
 361 __attribute__ ((section(".flashconfig"), used))
 362 const uint8_t flashconfigbytes[16] = {
 363         0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, // Backdoor Verif Key 28.3.1
 364
 365         //
 366         // Protecting the first 4k of Flash memory from being over-written while running (bootloader protection)
 367         // Still possible to overwrite the bootloader using an external flashing device
 368         // For more details see:
 369         //  http://cache.freescale.com/files/training/doc/dwf/AMF_ENT_T1031_Boston.pdf (page 8)
 370         //  http://cache.freescale.com/files/microcontrollers/doc/app_note/AN4507.pdf
 371         //  http://cache.freescale.com/files/32bit/doc/ref_manual/K20P48M50SF0RM.pdf (28.34.6)
 372         //
 373         0xFF, 0xFF, 0xFF, 0xFE, // Program Flash Protection Bytes FPROT0-3
 374
 375         0xBE, // Flash security byte FSEC
 376         0x03, // Flash nonvolatile option byte FOPT
 377         0xFF, // EEPROM Protection Byte FEPROT
 378         0xFF, // Data Flash Protection Byte FDPROT
 379 };
 380 #endif
 381
 382
 383
 384 // ----- Functions -----
 385
 386 __attribute__((noreturn))
 387 static inline void jump_to_app( uintptr_t addr )
 388 {
 389         // addr is in r0
 390         __asm__("ldr sp, [%[addr], #0]\n"
 391                 "ldr pc, [%[addr], #4]"
 392                 :: [addr] "r" (addr));
 393         // NOTREACHED
 394         __builtin_unreachable();
 395 }
 396
 397 void *memset( void *addr, int val, unsigned int len )
 398 {
 399         char *buf = addr;
 400
 401         for (; len > 0; --len, ++buf)
 402                 *buf = val;
 403         return (addr);
 404 }
 405
 406 int memcmp( const void *a, const void *b, unsigned int len )
 407 {
 408         const uint8_t *ap = a, *bp = b;
 409         int val = 0;
 410
 411         for (; len > 0 && (val = *ap - *bp) == 0; --len, ++ap, ++bp)
 412                 /* NOTHING */;
 413         return (val);
 414 }
 415
 416 void *memcpy( void *dst, const void *src, unsigned int len )
 417 {
 418         char *dstbuf = dst;
 419         const char *srcbuf = src;
 420
 421         for (; len > 0; --len, ++dstbuf, ++srcbuf)
 422                 *dstbuf = *srcbuf;
 423         return (dst);
 424 }
 425
 426
 427
 428 // ----- Chip Entry Point -----
 429
 430 __attribute__ ((section(".startup")))
 431 void ResetHandler()
 432 {
 433         // Disable Watchdog
 434         WDOG_UNLOCK = WDOG_UNLOCK_SEQ1;
 435         WDOG_UNLOCK = WDOG_UNLOCK_SEQ2;
 436         WDOG_STCTRLH = WDOG_STCTRLH_ALLOWUPDATE;
 437
 438 #if defined(_mk20dx128vlf5_) && defined(_bootloader_) // Bootloader Section
 439         extern uint32_t _app_rom;
 440
 441         // We treat _app_rom as pointer to directly read the stack
 442         // pointer and check for valid app code.  This is no fool
 443         // proof method, but it should help for the first flash.
 444         if ( RCM_SRS0 & 0x40 || _app_rom == 0xffffffff ||
 445           memcmp( (uint8_t*)&VBAT, sys_reset_to_loader_magic, sizeof(sys_reset_to_loader_magic) ) == 0 ) // Check for soft reload
 446         {
 447                 memset( (uint8_t*)&VBAT, 0, sizeof(VBAT) );
 448         }
 449         else
 450         {
 451                 uint32_t addr = (uintptr_t)&_app_rom;
 452                 SCB_VTOR = addr; // relocate vector table
 453                 jump_to_app( addr );
 454         }
 455 #endif
 456
 457         uint32_t *src = &_etext;
 458         uint32_t *dest = &_sdata;
 459
 460         // Enable clocks to always-used peripherals
 461         SIM_SCGC5 = 0x00043F82; // Clocks active to all GPIO
 462         SIM_SCGC6 = SIM_SCGC6_FTM0 | SIM_SCGC6_FTM1 | SIM_SCGC6_ADC0 | SIM_SCGC6_FTFL;
 463 #if defined(_mk20dx128_)
 464         SIM_SCGC6 |= SIM_SCGC6_RTC;
 465 #elif defined(_mk20dx256_)
 466         SIM_SCGC3 = SIM_SCGC3_ADC1 | SIM_SCGC3_FTM2;
 467         SIM_SCGC6 |= SIM_SCGC6_RTC;
 468 #endif
 469
 470 #if defined(_mk20dx128_) || defined(_mk20dx256_) // Teensy 3s
 471         // if the RTC oscillator isn't enabled, get it started early
 472         if ( !(RTC_CR & RTC_CR_OSCE) )
 473         {
 474                 RTC_SR = 0;
 475                 RTC_CR = RTC_CR_SC16P | RTC_CR_SC4P | RTC_CR_OSCE;
 476         }
 477 #endif
 478
 479         // release I/O pins hold, if we woke up from VLLS mode
 480         if ( PMC_REGSC & PMC_REGSC_ACKISO )
 481         {
 482                 PMC_REGSC |= PMC_REGSC_ACKISO;
 483         }
 484
 485         // Prepare RAM
 486         while ( dest < &_edata ) *dest++ = *src++;
 487         dest = &_sbss;
 488         while ( dest < &_ebss ) *dest++ = 0;
 489
 490 // MCHCK
 491 #if defined(_mk20dx128vlf5_)
 492         // Default all interrupts to medium priority level
 493         for ( unsigned int i = 0; i < NVIC_NUM_INTERRUPTS; i++ )
 494         {
 495                 NVIC_SET_PRIORITY( i, 128 );
 496         }
 497
 498         // FLL at 48MHz
 499         MCG_C4 = MCG_C4_DMX32 | MCG_C4_DRST_DRS( 1 );
 500
 501         // USB Clock and FLL select
 502         SIM_SOPT2 = SIM_SOPT2_USBSRC | SIM_SOPT2_TRACECLKSEL;
 503
 504 // Teensy 3.0 and 3.1
 505 #else
 506         unsigned int i;
 507
 508         SCB_VTOR = 0;   // use vector table in flash
 509
 510         // default all interrupts to medium priority level
 511         for ( i = 0; i < NVIC_NUM_INTERRUPTS; i++ )
 512         {
 513                 NVIC_SET_PRIORITY( i, 128 );
 514         }
 515
 516         // start in FEI mode
 517         // enable capacitors for crystal
 518         OSC0_CR = OSC_SC8P | OSC_SC2P;
 519
 520         // enable osc, 8-32 MHz range, low power mode
 521         MCG_C2 = MCG_C2_RANGE0( 2 ) | MCG_C2_EREFS;
 522
 523         // switch to crystal as clock source, FLL input = 16 MHz / 512
 524         MCG_C1 =  MCG_C1_CLKS( 2 ) | MCG_C1_FRDIV( 4 );
 525
 526         // wait for crystal oscillator to begin
 527         while ( (MCG_S & MCG_S_OSCINIT0) == 0 );
 528
 529         // wait for FLL to use oscillator
 530         while ( (MCG_S & MCG_S_IREFST) != 0 );
 531
 532         // wait for MCGOUT to use oscillator
 533         while ( (MCG_S & MCG_S_CLKST_MASK) != MCG_S_CLKST( 2 ) );
 534
 535         // now we're in FBE mode
 536         // config PLL input for 16 MHz Crystal / 4 = 4 MHz
 537         MCG_C5 = MCG_C5_PRDIV0( 3 );
 538
 539         // config PLL for 96 MHz output
 540         MCG_C6 = MCG_C6_PLLS | MCG_C6_VDIV0( 0 );
 541
 542         // wait for PLL to start using xtal as its input
 543         while ( !(MCG_S & MCG_S_PLLST) );
 544
 545         // wait for PLL to lock
 546         while ( !(MCG_S & MCG_S_LOCK0) );
 547
 548         // now we're in PBE mode
 549 #if F_CPU == 96000000
 550         // config divisors: 96 MHz core, 48 MHz bus, 24 MHz flash
 551         SIM_CLKDIV1 = SIM_CLKDIV1_OUTDIV1( 0 ) | SIM_CLKDIV1_OUTDIV2( 1 ) | SIM_CLKDIV1_OUTDIV4( 3 );
 552 #elif F_CPU == 48000000
 553         // config divisors: 48 MHz core, 48 MHz bus, 24 MHz flash
 554         SIM_CLKDIV1 = SIM_CLKDIV1_OUTDIV1( 1 ) | SIM_CLKDIV1_OUTDIV2( 1 ) | SIM_CLKDIV1_OUTDIV4( 3 );
 555 #elif F_CPU == 24000000
 556         // config divisors: 24 MHz core, 24 MHz bus, 24 MHz flash
 557         SIM_CLKDIV1 = SIM_CLKDIV1_OUTDIV1( 3 ) | SIM_CLKDIV1_OUTDIV2( 3 ) | SIM_CLKDIV1_OUTDIV4( 3 );
 558 #else
 559 #error "Error, F_CPU must be 96000000, 48000000, or 24000000"
 560 #endif
 561         // switch to PLL as clock source, FLL input = 16 MHz / 512
 562         MCG_C1 = MCG_C1_CLKS( 0 ) | MCG_C1_FRDIV( 4 );
 563
 564         // wait for PLL clock to be used
 565         while ( (MCG_S & MCG_S_CLKST_MASK) != MCG_S_CLKST( 3 ) );
 566
 567         // now we're in PEE mode
 568         // configure USB for 48 MHz clock
 569         SIM_CLKDIV2 = SIM_CLKDIV2_USBDIV( 1 ); // USB = 96 MHz PLL / 2
 570
 571         // USB uses PLL clock, trace is CPU clock, CLKOUT=OSCERCLK0
 572         SIM_SOPT2 = SIM_SOPT2_USBSRC | SIM_SOPT2_PLLFLLSEL | SIM_SOPT2_TRACECLKSEL | SIM_SOPT2_CLKOUTSEL( 6 );
 573
 574 #endif
 575
 576 #if !defined(_bootloader_)
 577         // Initialize the SysTick counter
 578         SYST_RVR = (F_CPU / 1000) - 1;
 579         SYST_CSR = SYST_CSR_CLKSOURCE | SYST_CSR_TICKINT | SYST_CSR_ENABLE;
 580
 581         __enable_irq();
 582 #else
 583         // Disable Watchdog for bootloader
 584         WDOG_STCTRLH &= ~WDOG_STCTRLH_WDOGEN;
 585 #endif
 586
 587         main();
 588         while ( 1 ); // Shouldn't get here...
 589 }
 590
 591
 592
 593 // ----- RAM Setup -----
 594
 595 char *__brkval = (char *)&_ebss;
 596
 597 void * _sbrk( int incr )
 598 {
 599         char *prev = __brkval;
 600         __brkval += incr;
 601         return prev;
 602 }
 603
 604
 605
 606 // ----- Interrupt Execution Priority -----
 607
 608 int nvic_execution_priority()
 609 {
 610         int priority = 256;
 611         uint32_t primask, faultmask, basepri, ipsr;
 612
 613         // full algorithm in ARM DDI0403D, page B1-639
 614         // this isn't quite complete, but hopefully good enough
 615         asm volatile( "mrs %0, faultmask\n" : "=r" (faultmask):: );
 616         if ( faultmask )
 617         {
 618                 return -1;
 619         }
 620
 621         asm volatile( "mrs %0, primask\n" : "=r" (primask):: );
 622         if ( primask )
 623         {
 624                 return 0;
 625         }
 626
 627         asm volatile( "mrs %0, ipsr\n" : "=r" (ipsr):: );
 628         if ( ipsr )
 629         {
 630                 if ( ipsr < 16)
 631                 {
 632                         priority = 0; // could be non-zero
 633                 }
 634                 else
 635                 {
 636                         priority = NVIC_GET_PRIORITY( ipsr - 16 );
 637                 }
 638         }
 639
 640         asm volatile( "mrs %0, basepri\n" : "=r" (basepri):: );
 641         if ( basepri > 0 && basepri < priority )
 642         {
 643                 priority = basepri;
 644         }
 645
 646         return priority;
 647 }
 648