3 files changed, 51 insertions, 2 deletions
diff --git a/lib/led.c b/lib/led.c
index 60436e2..ea5bcc3 100644
--- a/lib/led.c
+++ b/lib/led.c
@@ -20,7 +20,8 @@
  *  along with spreadspace avr utils. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "avr/io.h"
+#include <avr/io.h>
+
 #include "led.h"
 
 #if defined(__BOARD_arduinoUno__) || defined(__BOARD_AvrNetIo__)
diff --git a/lib/util.c b/lib/util.c
index 3bf17b1..76f5104 100644
--- a/lib/util.c
+++ b/lib/util.c
@@ -20,7 +20,10 @@
  *  along with spreadspace avr utils. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "avr/io.h"
+#include <avr/interrupt.h>
+#include <avr/io.h>
+#include <util/delay.h>
+
 #include "util.h"
 
 #if defined(CLKPR)
@@ -33,3 +36,47 @@ void cpu_init(void)
 {
   CPU_PRESCALE(0);
 }
+
+void reset2bootloader(void)
+{
+#if defined(__BOARD_teensy1__) || defined(__BOARD_teensy1pp__) || defined(__BOARD_teensy2__) || defined(__BOARD_teensy2pp__) || defined(__BOARD_hhd70dongle__)
+  cli();
+  // disable watchdog, if enabled
+  // disable all peripherals
+  UDCON = 1;
+  USBCON = (1<<FRZCLK);  // disable USB
+  UCSR1B = 0;
+  _delay_ms(5);
+  #if defined(__BOARD_teensy1__)
+    EIMSK = 0; PCICR = 0; SPCR = 0; ACSR = 0; EECR = 0;
+    TIMSK0 = 0; TIMSK1 = 0; UCSR1B = 0;
+    DDRB = 0; DDRC = 0; DDRD = 0;
+    PORTB = 0; PORTC = 0; PORTD = 0;
+    asm volatile("jmp 0x3E00");
+  #elif defined(__BOARD_teensy2__)
+    EIMSK = 0; PCICR = 0; SPCR = 0; ACSR = 0; EECR = 0; ADCSRA = 0;
+    TIMSK0 = 0; TIMSK1 = 0; TIMSK3 = 0; TIMSK4 = 0; UCSR1B = 0; TWCR = 0;
+    DDRB = 0; DDRC = 0; DDRD = 0; DDRE = 0; DDRF = 0; TWCR = 0;
+    PORTB = 0; PORTC = 0; PORTD = 0; PORTE = 0; PORTF = 0;
+    asm volatile("jmp 0x7E00");
+  #elif defined(__BOARD_teensy2pp__)
+    EIMSK = 0; PCICR = 0; SPCR = 0; ACSR = 0; EECR = 0; ADCSRA = 0;
+    TIMSK0 = 0; TIMSK1 = 0; TIMSK2 = 0; TIMSK3 = 0; UCSR1B = 0; TWCR = 0;
+    DDRA = 0; DDRB = 0; DDRC = 0; DDRD = 0; DDRE = 0; DDRF = 0;
+    PORTA = 0; PORTB = 0; PORTC = 0; PORTD = 0; PORTE = 0; PORTF = 0;
+    asm volatile("jmp 0xFC00");
+  #elif defined(__BOARD_teensy2pp__)
+    EIMSK = 0; PCICR = 0; SPCR = 0; ACSR = 0; EECR = 0; ADCSRA = 0;
+    TIMSK0 = 0; TIMSK1 = 0; TIMSK2 = 0; TIMSK3 = 0; UCSR1B = 0; TWCR = 0;
+    DDRA = 0; DDRB = 0; DDRC = 0; DDRD = 0; DDRE = 0; DDRF = 0;
+    PORTA = 0; PORTB = 0; PORTC = 0; PORTD = 0; PORTE = 0; PORTF = 0;
+    asm volatile("jmp 0x1FC00");
+  #elif defined(__BOARD_hhd70dongle__)
+    EIMSK = 0; PCICR = 0; SPCR = 0; ACSR = 0; EECR = 0; ADCSRA = 0;
+    TIMSK0 = 0; TIMSK1 = 0; TIMSK3 = 0; TIMSK4 = 0; UCSR1B = 0; TWCR = 0;
+    DDRB = 0; DDRC = 0; DDRD = 0; DDRE = 0; DDRF = 0; TWCR = 0;
+    PORTB = 0; PORTC = 0; PORTD = 0; PORTE = 0; PORTF = 0;
+    asm volatile("jmp 0x3800");
+  #endif
+#endif
+}
diff --git a/lib/util.h b/lib/util.h
index 15204b1..77afa2e 100644
--- a/lib/util.h
+++ b/lib/util.h
@@ -24,5 +24,6 @@
 #define SPREADSPACE_util_h_INCLUDED
 
 void cpu_init(void);
+void reset2bootloader(void);
 
 #endif