1 files changed, 30 insertions, 0 deletions

diff --git a/common/bootloader.c b/common/bootloader.c
index e45295cd5..612b94964 100644
--- a/common/bootloader.c
+++ b/common/bootloader.c
@@ -24,12 +24,22 @@
 
 void bootloader_jump(void) {
     cli();
+
+    //
+    //Teensy
+    //
+#if defined(__AVR_AT90USB162__) || defined(__AVR_ATmega32U4__) || defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__)
     // disable watchdog, if enabled
     // disable all peripherals
     UDCON = 1;
     USBCON = (1<<FRZCLK);  // disable USB
     UCSR1B = 0;
     _delay_ms(5);
+#else
+    // This makes custom USBasploader come up.
+    MCUSR = 0;
+#endif
+
 #if defined(__AVR_AT90USB162__)
     EIMSK = 0; PCICR = 0; SPCR = 0; ACSR = 0; EECR = 0;
     TIMSK0 = 0; TIMSK1 = 0; UCSR1B = 0;
@@ -52,6 +62,26 @@ void bootloader_jump(void) {
     PORTA = 0; PORTB = 0; PORTC = 0; PORTD = 0; PORTE = 0; PORTF = 0;
 #endif
 
+
+    //
+    //USBasp
+    //
+#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega168P__) || defined(__AVR_ATmega328P)
+    // This makes custom USBasploader come up.
+    MCUSR = 0;
+
+    // initialize ports
+    PORTB = 0; PORTC= 0; PORTD = 0;
+    DDRB = 0; DDRC= 0; DDRD = 0;
+
+    // disable interrupts
+    EIMSK = 0; EECR = 0; SPCR = 0;
+    ACSR = 0; SPMCSR = 0; WDTCSR = 0; PCICR = 0;
+    TIMSK0 = 0; TIMSK1 = 0; TIMSK2 = 0;
+    ADCSRA = 0; TWCR = 0; UCSR0B = 0;
+#endif
+
+
     // start Bootloader
     ((void (*)(void))BOOTLOADER_START)();
 }