Working on NXP/Teensy support

11 files changed, 449 insertions, 84 deletions

diff --git a/src/device/arm/arm.go b/src/device/arm/arm.go
index 45226dab4..d7ccaab7e 100644
--- a/src/device/arm/arm.go
+++ b/src/device/arm/arm.go
@@ -174,6 +174,11 @@ func EnableIRQ(irq uint32) {
 	NVIC.ISER[irq>>5].Set(1 << (irq & 0x1F))
 }
 
+// Disable the given interrupt number.
+func DisableIRQ(irq uint32) {
+	NVIC.ICER[irq>>5].Set(1 << (irq & 0x1F))
+}
+
 // Set the priority of the given interrupt number.
 // Note that the priority is given as a 0-255 number, where some of the lower
 // bits are not implemented by the hardware. For example, to set a low interrupt
diff --git a/src/machine/board_teensy36.go b/src/machine/board_teensy36.go
index 1b78677b1..f7991af7b 100644
--- a/src/machine/board_teensy36.go
+++ b/src/machine/board_teensy36.go
@@ -6,12 +6,12 @@ import (
 	"device/nxp"
 )
 
-//go:keep
-//go:section .flashconfig
-var FlashConfig = [16]byte{
-	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
-	0xFF, 0xFF, 0xFF, 0xFF, 0xDE, 0xF9, 0xFF, 0xFF,
-}
+// //go:keep
+// //go:section .flash_config
+// var FlashControl = [16]byte{
+// 	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+// 	0xFF, 0xFF, 0xFF, 0xFF, 0xDE, 0xF9, 0xFF, 0xFF,
+// }
 
 func CPUFrequency() uint32 {
 	return 180000000
@@ -20,20 +20,73 @@ func CPUFrequency() uint32 {
 // LED on the Teensy
 const LED Pin = 13
 
-var _pinRegisters [64]pinRegisters
-
-func init() {
-	_pinRegisters[13].Bit = 5
-	_pinRegisters[13].PCR = &nxp.PORTC.PCR5
-	_pinRegisters[13].PDOR = nxp.GPIOC.PDOR.Bit(5)
-	_pinRegisters[13].PSOR = nxp.GPIOC.PSOR.Bit(5)
-	_pinRegisters[13].PCOR = nxp.GPIOC.PCOR.Bit(5)
-	_pinRegisters[13].PTOR = nxp.GPIOC.PTOR.Bit(5)
-	_pinRegisters[13].PDIR = nxp.GPIOC.PDIR.Bit(5)
-	_pinRegisters[13].PDDR = nxp.GPIOC.PDDR.Bit(5)
+var pins = []pin{
+	// {bit, control register, gpio register bank}
+	0:  {16, &nxp.PORTB.PCR16, nxp.GPIOB},
+	1:  {17, &nxp.PORTB.PCR17, nxp.GPIOB},
+	2:  {0, &nxp.PORTD.PCR0, nxp.GPIOD},
+	3:  {12, &nxp.PORTA.PCR12, nxp.GPIOA},
+	4:  {13, &nxp.PORTA.PCR13, nxp.GPIOA},
+	5:  {7, &nxp.PORTD.PCR7, nxp.GPIOD},
+	6:  {4, &nxp.PORTD.PCR4, nxp.GPIOD},
+	7:  {2, &nxp.PORTD.PCR2, nxp.GPIOD},
+	8:  {3, &nxp.PORTD.PCR3, nxp.GPIOD},
+	9:  {3, &nxp.PORTC.PCR3, nxp.GPIOC},
+	10: {4, &nxp.PORTC.PCR4, nxp.GPIOC},
+	11: {6, &nxp.PORTC.PCR6, nxp.GPIOC},
+	12: {7, &nxp.PORTC.PCR7, nxp.GPIOC},
+	13: {5, &nxp.PORTC.PCR5, nxp.GPIOC},
+	14: {1, &nxp.PORTD.PCR1, nxp.GPIOD},
+	15: {0, &nxp.PORTC.PCR0, nxp.GPIOC},
+	16: {0, &nxp.PORTB.PCR0, nxp.GPIOB},
+	17: {1, &nxp.PORTB.PCR1, nxp.GPIOB},
+	18: {3, &nxp.PORTB.PCR3, nxp.GPIOB},
+	19: {2, &nxp.PORTB.PCR2, nxp.GPIOB},
+	20: {5, &nxp.PORTD.PCR5, nxp.GPIOD},
+	21: {6, &nxp.PORTD.PCR6, nxp.GPIOD},
+	22: {1, &nxp.PORTC.PCR1, nxp.GPIOC},
+	23: {2, &nxp.PORTC.PCR2, nxp.GPIOC},
+	24: {26, &nxp.PORTE.PCR26, nxp.GPIOE},
+	25: {5, &nxp.PORTA.PCR5, nxp.GPIOA},
+	26: {14, &nxp.PORTA.PCR14, nxp.GPIOA},
+	27: {15, &nxp.PORTA.PCR15, nxp.GPIOA},
+	28: {16, &nxp.PORTA.PCR16, nxp.GPIOA},
+	29: {18, &nxp.PORTB.PCR18, nxp.GPIOB},
+	30: {19, &nxp.PORTB.PCR19, nxp.GPIOB},
+	31: {10, &nxp.PORTB.PCR10, nxp.GPIOB},
+	32: {11, &nxp.PORTB.PCR11, nxp.GPIOB},
+	33: {24, &nxp.PORTE.PCR24, nxp.GPIOE},
+	34: {25, &nxp.PORTE.PCR25, nxp.GPIOE},
+	35: {8, &nxp.PORTC.PCR8, nxp.GPIOC},
+	36: {9, &nxp.PORTC.PCR9, nxp.GPIOC},
+	37: {10, &nxp.PORTC.PCR10, nxp.GPIOC},
+	38: {11, &nxp.PORTC.PCR11, nxp.GPIOC},
+	39: {17, &nxp.PORTA.PCR17, nxp.GPIOA},
+	40: {28, &nxp.PORTA.PCR28, nxp.GPIOA},
+	41: {29, &nxp.PORTA.PCR29, nxp.GPIOA},
+	42: {26, &nxp.PORTA.PCR26, nxp.GPIOA},
+	43: {20, &nxp.PORTB.PCR20, nxp.GPIOB},
+	44: {22, &nxp.PORTB.PCR22, nxp.GPIOB},
+	45: {23, &nxp.PORTB.PCR23, nxp.GPIOB},
+	46: {21, &nxp.PORTB.PCR21, nxp.GPIOB},
+	47: {8, &nxp.PORTD.PCR8, nxp.GPIOD},
+	48: {9, &nxp.PORTD.PCR9, nxp.GPIOD},
+	49: {4, &nxp.PORTB.PCR4, nxp.GPIOB},
+	50: {5, &nxp.PORTB.PCR5, nxp.GPIOB},
+	51: {14, &nxp.PORTD.PCR14, nxp.GPIOD},
+	52: {13, &nxp.PORTD.PCR13, nxp.GPIOD},
+	53: {12, &nxp.PORTD.PCR12, nxp.GPIOD},
+	54: {15, &nxp.PORTD.PCR15, nxp.GPIOD},
+	55: {11, &nxp.PORTD.PCR11, nxp.GPIOD},
+	56: {10, &nxp.PORTE.PCR10, nxp.GPIOE},
+	57: {11, &nxp.PORTE.PCR11, nxp.GPIOE},
+	58: {0, &nxp.PORTE.PCR0, nxp.GPIOE},
+	59: {1, &nxp.PORTE.PCR1, nxp.GPIOE},
+	60: {2, &nxp.PORTE.PCR2, nxp.GPIOE},
+	61: {3, &nxp.PORTE.PCR3, nxp.GPIOE},
+	62: {4, &nxp.PORTE.PCR4, nxp.GPIOE},
+	63: {5, &nxp.PORTE.PCR5, nxp.GPIOE},
 }
 
 //go:inline
-func (p Pin) registers() pinRegisters {
-	return _pinRegisters[p]
-}
+func (p Pin) reg() pin { return pins[p] }
diff --git a/src/machine/buffer.go b/src/machine/buffer.go
index ee12b9bcc..1528c25be 100644
--- a/src/machine/buffer.go
+++ b/src/machine/buffer.go
@@ -44,3 +44,9 @@ func (rb *RingBuffer) Get() (byte, bool) {
 	}
 	return 0, false
 }
+
+// Clear resets the head and tail pointer to zero.
+func (rb *RingBuffer) Clear() {
+	rb.head.Set(0)
+	rb.tail.Set(0)
+}
diff --git a/src/machine/machine_nxpmk66f18.go b/src/machine/machine_nxpmk66f18.go
index ab15ed136..291fe1db7 100644
--- a/src/machine/machine_nxpmk66f18.go
+++ b/src/machine/machine_nxpmk66f18.go
@@ -7,19 +7,7 @@ import (
 	"runtime/volatile"
 )
 
-const (
-	PortControlRegisterSRE = nxp.PORT_PCR0_SRE
-	PortControlRegisterDSE = nxp.PORT_PCR0_DSE
-	PortControlRegisterODE = nxp.PORT_PCR0_ODE
-)
-
-func PortControlRegisterMUX(v uint8) uint32 {
-	return (uint32(v) << nxp.PORT_PCR0_MUX_Pos) & nxp.PORT_PCR0_MUX_Msk
-}
-
-type pinRegisters struct {
-	Bit  uintptr
-	PCR  *volatile.Register32
+type FastPin struct {
 	PDOR *volatile.BitRegister
 	PSOR *volatile.BitRegister
 	PCOR *volatile.BitRegister
@@ -28,6 +16,12 @@ type pinRegisters struct {
 	PDDR *volatile.BitRegister
 }
 
+type pin struct {
+	Bit  uint8
+	PCR  *volatile.Register32
+	GPIO *nxp.GPIO_Type
+}
+
 // Configure this pin with the given configuration.
 func (p Pin) Configure(config PinConfig) {
 	switch config.Mode {
@@ -35,22 +29,47 @@ func (p Pin) Configure(config PinConfig) {
 		panic("todo")
 
 	case PinOutput:
-		p.registers().PDDR.Set()
-		p.registers().PCR.SetBits(PortControlRegisterSRE | PortControlRegisterDSE | PortControlRegisterMUX(1))
-		p.registers().PCR.ClearBits(PortControlRegisterODE)
+		r := p.reg()
+		r.GPIO.PDDR.SetBits(1 << r.Bit)
+		r.PCR.SetBits(nxp.PORT_PCR0_SRE | nxp.PORT_PCR0_DSE | nxp.PORT_PCR0_MUX(1))
+		r.PCR.ClearBits(nxp.PORT_PCR0_ODE)
 	}
 }
 
 // Set changes the value of the GPIO pin. The pin must be configured as output.
 func (p Pin) Set(value bool) {
+	r := p.reg()
 	if value {
-		p.registers().PSOR.Set()
+		r.GPIO.PSOR.Set(1 << r.Bit)
 	} else {
-		p.registers().PCOR.Set()
+		r.GPIO.PCOR.Set(1 << r.Bit)
 	}
 }
 
 // Get returns the current value of a GPIO pin.
 func (p Pin) Get() bool {
-	return p.registers().PDIR.Get()
+	r := p.reg()
+	return r.GPIO.PDIR.HasBits(1 << r.Bit)
+}
+
+func (p Pin) Control() *volatile.Register32 {
+	return p.reg().PCR
 }
+
+func (p Pin) Fast() FastPin {
+	r := p.reg()
+	return FastPin{
+		PDOR: r.GPIO.PDOR.Bit(r.Bit),
+		PSOR: r.GPIO.PSOR.Bit(r.Bit),
+		PCOR: r.GPIO.PCOR.Bit(r.Bit),
+		PTOR: r.GPIO.PTOR.Bit(r.Bit),
+		PDIR: r.GPIO.PDIR.Bit(r.Bit),
+		PDDR: r.GPIO.PDDR.Bit(r.Bit),
+	}
+}
+
+func (p FastPin) Set()         { p.PSOR.Set(true) }
+func (p FastPin) Clear()       { p.PCOR.Set(true) }
+func (p FastPin) Toggle()      { p.PTOR.Set(true) }
+func (p FastPin) Write(v bool) { p.PDOR.Set(v) }
+func (p FastPin) Read() bool   { return p.PDIR.Get() }
diff --git a/src/machine/uart_nxpmk66f18.go b/src/machine/uart_nxpmk66f18.go
new file mode 100644
index 000000000..f5cc0b182
--- /dev/null
+++ b/src/machine/uart_nxpmk66f18.go
@@ -0,0 +1,251 @@
+// +build nxp,mk66f18
+
+package machine
+
+import (
+	"device/arm"
+	"device/nxp"
+	"errors"
+	"runtime/volatile"
+
+	_ "unsafe" // for go:linkname
+)
+
+const (
+	uartC2Enable       = nxp.UART_C2_TE | nxp.UART_C2_RE | nxp.UART_C2_RIE | nxp.UART_C2_ILIE
+	uartC2TXActive     = uartC2Enable | nxp.UART_C2_TIE
+	uartC2TXCompleting = uartC2Enable | nxp.UART_C2_TCIE
+	uartC2TXInactive   = uartC2Enable
+
+	uartIRQPriority = 64
+)
+
+var (
+	ErrNotImplemented = errors.New("device has not been implemented")
+	ErrNotConfigured  = errors.New("device has not been configured")
+)
+
+type UARTConfig struct {
+	BaudRate uint32
+}
+
+type UART struct {
+	*nxp.UART_Type
+	RXPCR     *volatile.Register32
+	TXPCR     *volatile.Register32
+	SCGC      *volatile.Register32
+	SCGCMask  uint32
+	IRQNumber uint32
+
+	// state
+	RXBuffer     RingBuffer
+	TXBuffer     RingBuffer
+	Transmitting volatile.Register8
+}
+
+// 'UART0' in the K66 manual corresponds to 'UART1' on the Teensy's pinout
+var UART1 = UART{UART_Type: nxp.UART0, RXPCR: pins[0].PCR, TXPCR: pins[1].PCR, SCGC: &nxp.SIM.SCGC4, SCGCMask: nxp.SIM_SCGC4_UART0, IRQNumber: nxp.IRQ_UART0_RX_TX}
+var UART2 = UART{UART_Type: nxp.UART1, RXPCR: pins[9].PCR, TXPCR: pins[10].PCR, SCGC: &nxp.SIM.SCGC4, SCGCMask: nxp.SIM_SCGC4_UART1, IRQNumber: nxp.IRQ_UART1_RX_TX}
+var UART3 = UART{UART_Type: nxp.UART2, RXPCR: pins[7].PCR, TXPCR: pins[8].PCR, SCGC: &nxp.SIM.SCGC4, SCGCMask: nxp.SIM_SCGC4_UART2, IRQNumber: nxp.IRQ_UART2_RX_TX}
+var UART4 = UART{UART_Type: nxp.UART3, RXPCR: pins[31].PCR, TXPCR: pins[32].PCR, SCGC: &nxp.SIM.SCGC4, SCGCMask: nxp.SIM_SCGC4_UART3, IRQNumber: nxp.IRQ_UART3_RX_TX}
+var UART5 = UART{UART_Type: nxp.UART4, RXPCR: pins[34].PCR, TXPCR: pins[33].PCR, SCGC: &nxp.SIM.SCGC1, SCGCMask: nxp.SIM_SCGC1_UART4, IRQNumber: nxp.IRQ_UART4_RX_TX}
+
+//go:export UART0_RX_TX_IRQHandler
+func uart0StatusISR() { UART1.handleStatusInterrupt() }
+
+//go:export UART1_RX_TX_IRQHandler
+func uart1StatusISR() { UART2.handleStatusInterrupt() }
+
+//go:export UART2_RX_TX_IRQHandler
+func uart2StatusISR() { UART3.handleStatusInterrupt() }
+
+//go:export UART3_RX_TX_IRQHandler
+func uart3StatusISR() { UART4.handleStatusInterrupt() }
+
+//go:export UART4_RX_TX_IRQHandler
+func uart4StatusISR() { UART5.handleStatusInterrupt() }
+
+// Configure the UART.
+func (u *UART) Configure(config UARTConfig) {
+	en := u.SCGC.HasBits(u.SCGCMask)
+
+	// adapted from Teensy core's serial_begin
+
+	if !en {
+		u.Transmitting.Set(0)
+
+		// turn on the clock
+		u.SCGC.Set(u.SCGCMask)
+
+		// configure pins
+		u.RXPCR.Set(nxp.PORT_PCR0_PE | nxp.PORT_PCR0_PS | nxp.PORT_PCR0_PFE | nxp.PORT_PCR0_MUX(3))
+		u.TXPCR.Set(nxp.PORT_PCR0_DSE | nxp.PORT_PCR0_SRE | nxp.PORT_PCR0_MUX(3))
+		u.C1.Set(nxp.UART_C1_ILT)
+	}
+
+	// default to 115200 baud
+	if config.BaudRate == 0 {
+		config.BaudRate = 115200
+	}
+
+	// copied from teensy core's BAUD2DIV macro
+	divisor := ((CPUFrequency() * 2) + ((config.BaudRate) >> 1)) / config.BaudRate
+	if divisor < 32 {
+		divisor = 32
+	}
+
+	if en {
+		// don't change baud rate mid transmit
+		u.Flush()
+	}
+
+	// set the divisor
+	u.BDH.Set(uint8((divisor >> 13) & 0x1F))
+	u.BDL.Set(uint8((divisor >> 5) & 0xFF))
+	u.C4.Set(uint8(divisor & 0x1F))
+
+	if !en {
+		u.C1.Set(nxp.UART_C1_ILT)
+
+		// configure TX and RX watermark
+		u.TWFIFO.Set(2) // causes bit TDRE of S1 to set
+		u.RWFIFO.Set(4) // causes bit RDRF of S1 to set
+
+		u.PFIFO.Set(nxp.UART_PFIFO_TXFE | nxp.UART_PFIFO_RXFE)
+		u.C2.Set(uartC2TXInactive)
+
+		arm.SetPriority(u.IRQNumber, uartIRQPriority)
+		arm.EnableIRQ(u.IRQNumber)
+	}
+}
+
+func (u *UART) Disable() {
+	// adapted from Teensy core's serial_end
+
+	// check if the device has been enabled already
+	if !u.SCGC.HasBits(u.SCGCMask) {
+		return
+	}
+
+	u.Flush()
+
+	arm.DisableIRQ(u.IRQNumber)
+	u.C2.Set(0)
+
+	// reconfigure pin
+	u.RXPCR.Set(nxp.PORT_PCR0_PE | nxp.PORT_PCR0_PS | nxp.PORT_PCR0_MUX(1))
+	u.TXPCR.Set(nxp.PORT_PCR0_PE | nxp.PORT_PCR0_PS | nxp.PORT_PCR0_MUX(1))
+
+	// clear flags
+	u.S1.Get()
+	u.D.Get()
+	u.RXBuffer.Clear()
+}
+
+func (u *UART) Flush() {
+	for u.Transmitting.Get() != 0 {
+		// gosched()
+	}
+}
+
+// adapted from Teensy core's uart0_status_isr
+func (u *UART) handleStatusInterrupt() {
+	if u.S1.HasBits(nxp.UART_S1_RDRF | nxp.UART_S1_IDLE) {
+		intrs := arm.DisableInterrupts()
+		avail := u.RCFIFO.Get()
+		if avail == 0 {
+			// The only way to clear the IDLE interrupt flag is
+			// to read the data register.  But reading with no
+			// data causes a FIFO underrun, which causes the
+			// FIFO to return corrupted data.  If anyone from
+			// Freescale reads this, what a poor design!  There
+			// write should be a write-1-to-clear for IDLE.
+			u.D.Get()
+			// flushing the fifo recovers from the underrun,
+			// but there's a possible race condition where a
+			// new character could be received between reading
+			// RCFIFO == 0 and flushing the FIFO.  To minimize
+			// the chance, interrupts are disabled so a higher
+			// priority interrupt (hopefully) doesn't delay.
+			// TODO: change this to disabling the IDLE interrupt
+			// which won't be simple, since we already manage
+			// which transmit interrupts are enabled.
+			u.CFIFO.Set(nxp.UART_CFIFO_RXFLUSH)
+			arm.EnableInterrupts(intrs)
+
+		} else {
+			arm.EnableInterrupts(intrs)
+
+			for {
+				u.RXBuffer.Put(u.D.Get())
+				avail--
+				if avail <= 0 {
+					break
+				}
+			}
+		}
+	}
+
+	c := u.C2.Get()
+	if c&nxp.UART_C2_TIE != 0 && u.S1.HasBits(nxp.UART_S1_TDRE) {
+		for {
+			n, ok := u.TXBuffer.Get()
+			if !ok {
+				break
+			}
+
+			u.S1.Get()
+			u.D.Set(n)
+
+			if u.TCFIFO.Get() >= 8 {
+				break
+			}
+		}
+
+		if u.S1.HasBits(nxp.UART_S1_TDRE) {
+			u.Transmitting.Set(0)
+			u.C2.Set(uartC2TXCompleting)
+		}
+	}
+
+	if c&nxp.UART_C2_TCIE != 0 && u.S1.HasBits(nxp.UART_S1_TC) {
+		u.C2.Set(uartC2TXInactive)
+	}
+}
+
+//go:linkname gosched runtime.Gosched
+func gosched()
+
+// WriteByte writes a byte of data to the UART.
+func (u *UART) WriteByte(c byte) error {
+	if !u.SCGC.HasBits(u.SCGCMask) {
+		return ErrNotConfigured
+	}
+
+	for !u.S1.HasBits(nxp.UART_S1_TDRE) {
+		gosched()
+	}
+	u.D.Set(c)
+
+	// // wait for room on the buffer
+	// for !u.TXBuffer.Put(c) {
+	// 	gosched()
+	// }
+
+	// var wrote bool
+	// for u.S1.HasBits(nxp.UART_S1_TDRE) {
+	// 	n, ok := u.TXBuffer.Get()
+	// 	if ok {
+	// 		u.D.Set(n)
+	// 		wrote = true
+	// 	} else {
+	// 		break
+	// 	}
+	// }
+
+	// if wrote {
+	// 	u.Transmitting.Set(1)
+	// 	u.C2.Set(uartC2TXActive)
+	// }
+	return nil
+}
diff --git a/src/runtime/interrupt/interrupt_cortexm.go b/src/runtime/interrupt/interrupt_cortexm.go
index c23f736d8..0653cb69a 100644
--- a/src/runtime/interrupt/interrupt_cortexm.go
+++ b/src/runtime/interrupt/interrupt_cortexm.go
@@ -12,6 +12,11 @@ func (irq Interrupt) Enable() {
 	arm.EnableIRQ(uint32(irq.num))
 }
 
+// Disable disables this interrupt.
+func (irq Interrupt) Disable() {
+	arm.DisableIRQ(uint32(irq.num))
+}
+
 // SetPriority sets the interrupt priority for this interrupt. A lower number
 // means a higher priority. Additionally, most hardware doesn't implement all
 // priority bits (only the uppoer bits).
diff --git a/src/runtime/runtime_nxpmk66f18.go b/src/runtime/runtime_nxpmk66f18.go
index 99617a402..fef738f42 100644
--- a/src/runtime/runtime_nxpmk66f18.go
+++ b/src/runtime/runtime_nxpmk66f18.go
@@ -52,9 +52,31 @@ var (
 	SMC_PMSTAT_HSRUN   = nxp.SMC_PMSTAT_PMSTAT(0x80)
 )
 
+var bootMsg = []byte("\r\n\r\nStartup complete, running main\r\n\r\n")
+
 //go:section .resetHandler
 //go:export Reset_Handler
 func main() {
+	initSystem()
+	arm.Asm("CPSIE i")
+	initInternal()
+	startupLateHook()
+
+	initAll()
+	machine.UART1.Configure(machine.UARTConfig{BaudRate: 115200})
+	for _, c := range bootMsg {
+		for !machine.UART1.S1.HasBits(nxp.UART_S1_TDRE) {
+		}
+		machine.UART1.D.Set(c)
+	}
+
+	callMain()
+	abort()
+}
+
+// ported ResetHandler from mk20dx128.c from teensy3 core libraries
+//go:noinline
+func initSystem() {
 	nxp.WDOG.UNLOCK.Set(WDOG_UNLOCK_SEQ1)
 	nxp.WDOG.UNLOCK.Set(WDOG_UNLOCK_SEQ2)
 	arm.Asm("nop")
@@ -156,22 +178,11 @@ func main() {
 	nxp.SysTick.CVR.Set(0)
 	nxp.SysTick.CSR.Set(nxp.SysTick_CSR_CLKSOURCE | nxp.SysTick_CSR_TICKINT | nxp.SysTick_CSR_ENABLE)
 	nxp.SystemControl.SHPR3.Set(0x20200000) // Systick = priority 32
-
-	arm.Asm("CPSIE i")
-	initTeensyInternal()
-	startupLateHook()
-
-	// initAll()
-	runMain()
-	// abort()
-
-	for {
-
-	}
 }
 
 // ported _init_Teensyduino_internal_ from pins_teensy.c from teensy3 core libraries
-func initTeensyInternal() {
+//go:noinline
+func initInternal() {
 	arm.EnableIRQ(nxp.IRQ_PORTA)
 	arm.EnableIRQ(nxp.IRQ_PORTB)
 	arm.EnableIRQ(nxp.IRQ_PORTC)
@@ -225,6 +236,11 @@ func initTeensyInternal() {
 	nxp.TPM1.C1SC.Set(0x28)
 	nxp.TPM1.SC.Set(nxp.FTM_SC_CLKS(1) | nxp.FTM_SC_PS(0))
 
+	// configure the low-power timer
+	// nxp.LPTMR0.CSR.Set(nxp.LPTMR0_CSR_TIE)
+	// nxp.LPTMR0.PSR.Set(nxp.LPTMR0_PSR_PCS(3) | nxp.LPTMR0_PSR_PRESCALE(1)) // use main (external) clock, divided by 4
+	// arm.EnableIRQ(nxp.IRQ_LPTMR0)
+
 	// 	analog_init();
 
 	// #if !defined(TEENSY_INIT_USB_DELAY_BEFORE)
@@ -264,13 +280,8 @@ func startupLateHook() {
 	// TODO allow override
 }
 
-//go:noinline
-func runMain() {
-	// this is a separate function to ensure that Reset_Handler fits in 0x230 bytes regardless of whether (user) main requires scheduling
-	callMain()
-}
-
 func putchar(c byte) {
+	machine.UART1.WriteByte(c)
 }
 
 // ???
@@ -280,18 +291,18 @@ const asyncScheduler = false
 const tickMicros = 1000
 
 // number of ticks since boot
-var tickMilliCount uint32
+var tickMilliCount volatile.Register32
 
 //go:export SysTick_Handler
 func tickHandler() {
-	volatile.StoreUint32(&tickMilliCount, volatile.LoadUint32(&tickMilliCount)+1)
+	tickMilliCount.Set(tickMilliCount.Get() + 1)
 }
 
 // ticks are in microseconds
 func ticks() timeUnit {
 	m := arm.DisableInterrupts()
 	current := nxp.SysTick.CVR.Get()
-	count := tickMilliCount
+	count := tickMilliCount.Get()
 	istatus := nxp.SystemControl.ICSR.Get()
 	arm.EnableInterrupts(m)
 
@@ -322,7 +333,7 @@ func sleepTicks(d timeUnit) {
 			}
 			start += 1000
 		}
-		// Gosched()
+		arm.Asm("wfi")
 	}
 }
 
diff --git a/src/runtime/volatile/register_nxpmk66f18.go b/src/runtime/volatile/register_nxpmk66f18.go
index 272205a4d..0f9b4cb80 100644
--- a/src/runtime/volatile/register_nxpmk66f18.go
+++ b/src/runtime/volatile/register_nxpmk66f18.go
@@ -10,14 +10,14 @@ const bitbandBase = 0x42000000
 const ptrBytes = unsafe.Sizeof(uintptr(0))
 
 //go:inline
-func bitbandAddress(reg uintptr, bit uintptr) uintptr {
-	if bit > ptrBytes*8 {
+func bitbandAddress(reg uintptr, bit uint8) uintptr {
+	if uintptr(bit) > ptrBytes*8 {
 		panic("invalid bit position")
 	}
 	if reg < registerBase || reg >= registerEnd {
 		panic("register is out of range")
 	}
-	return (reg-registerBase)*ptrBytes*8 + bit*ptrBytes + bitbandBase
+	return (reg-registerBase)*ptrBytes*8 + uintptr(bit)*ptrBytes + bitbandBase
 }
 
 // Special types that causes loads/stores to be volatile (necessary for
@@ -40,17 +40,12 @@ func (r *BitRegister) Get() bool {
 //     *r.Reg = 1
 //
 //go:inline
-func (r *BitRegister) Set() {
-	StoreUint32(&r.Reg, 1)
-}
-
-// Clear clears the mapped register bit. It is the volatile equivalent of:
-//
-//     *r.Reg = 0
-//
-//go:inline
-func (r *BitRegister) Clear() {
-	StoreUint32(&r.Reg, 0)
+func (r *BitRegister) Set(v bool) {
+	var i uint32
+	if v {
+		i = 1
+	}
+	StoreUint32(&r.Reg, i)
 }
 
 // Bit maps bit N of register R to the corresponding bitband address. Bit panics
@@ -58,7 +53,7 @@ func (r *BitRegister) Clear() {
 // the number of bits in a register minus one).
 //
 // go:inline
-func (r *Register8) Bit(bit uintptr) *BitRegister {
+func (r *Register8) Bit(bit uint8) *BitRegister {
 	ptr := bitbandAddress(uintptr(unsafe.Pointer(&r.Reg)), bit)
 	return (*BitRegister)(unsafe.Pointer(ptr))
 }
@@ -68,7 +63,7 @@ func (r *Register8) Bit(bit uintptr) *BitRegister {
 // the number of bits in a register minus one).
 //
 // go:inline
-func (r *Register16) Bit(bit uintptr) *BitRegister {
+func (r *Register16) Bit(bit uint8) *BitRegister {
 	ptr := bitbandAddress(uintptr(unsafe.Pointer(&r.Reg)), bit)
 	return (*BitRegister)(unsafe.Pointer(ptr))
 }
@@ -78,7 +73,7 @@ func (r *Register16) Bit(bit uintptr) *BitRegister {
 // the number of bits in a register minus one).
 //
 // go:inline
-func (r *Register32) Bit(bit uintptr) *BitRegister {
+func (r *Register32) Bit(bit uint8) *BitRegister {
 	ptr := bitbandAddress(uintptr(unsafe.Pointer(&r.Reg)), bit)
 	return (*BitRegister)(unsafe.Pointer(ptr))
 }
diff --git a/targets/nxpmk66f18.ld b/targets/nxpmk66f18.ld
index 74e22761f..3487264e6 100644
--- a/targets/nxpmk66f18.ld
+++ b/targets/nxpmk66f18.ld
@@ -22,19 +22,19 @@ SECTIONS
         _vector_table = .;
         KEEP(*(.isr_vector))
 
-        /* this works as long as reset handler doesn't overflow past 0x400 */
-        *(.resetHandler)
-
         /* flash configuration MUST be at 0x400 */
         . = 0x400;
-		KEEP(*(.flashconfig))
+        _flash_config = .;
+		KEEP(*(.flash_config))
 
         /* everything else */
+        *(.resetHandler)
         *(.text)
         *(.text*)
         *(.rodata)
         *(.rodata*)
         . = ALIGN(4);
+
     } >FLASH_TEXT = 0xFF
 
     /* Put the stack at the bottom of RAM, so that the application will
diff --git a/targets/teensy36.json b/targets/teensy36.json
index fecd3c357..473aec2eb 100644
--- a/targets/teensy36.json
+++ b/targets/teensy36.json
@@ -11,7 +11,8 @@
 	],
 	"linkerscript": "targets/nxpmk66f18.ld",
 	"extra-files": [
-		"src/device/nxp/mk66f18.s"
+		"src/device/nxp/mk66f18.s",
+		"targets/teensy36.s"
 	],
 	"flash-command": "teensy_loader_cli -mmcu=mk66fx1m0 -v -w {hex}"
 }
diff --git a/targets/teensy36.s b/targets/teensy36.s
new file mode 100644
index 000000000..4e17e7c9d
--- /dev/null
+++ b/targets/teensy36.s
@@ -0,0 +1,19 @@
+
+.section .flash_config
+.global  __flash_config
+	.byte 0xFF
+    .byte 0xFF
+    .byte 0xFF
+    .byte 0xFF
+    .byte 0xFF
+    .byte 0xFF
+    .byte 0xFF
+    .byte 0xFF
+	.byte 0xFF
+    .byte 0xFF
+    .byte 0xFF
+    .byte 0xFF
+    .byte 0xDE
+    .byte 0xF9
+    .byte 0xFF
+    .byte 0xFF
\ No newline at end of file