From c0cb91de76c2223b560b529034ac075ba4d7f55a Mon Sep 17 00:00:00 2001
From: Adam Spickard <aspickard@novetta.com>
Date: Mon, 22 Feb 2016 13:39:12 -0500
Subject: [PATCH] Fix for Teensy RESET key.

---
 keyboard/ergodox_ez/config.h |  2 ++
 quantum/keymap_common.c      | 14 ++++++++++++++
 2 files changed, 16 insertions(+)

diff --git a/keyboard/ergodox_ez/config.h b/keyboard/ergodox_ez/config.h
index 825997ba05eb..0fb1852bf467 100644
--- a/keyboard/ergodox_ez/config.h
+++ b/keyboard/ergodox_ez/config.h
@@ -59,6 +59,8 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define LOCKING_SUPPORT_ENABLE
 /* Locking resynchronize hack */
 #define LOCKING_RESYNC_ENABLE
+/* Must declare the TEENSY chip to have RESET key available. */
+#define TEENSY
 
 /* key combination for command */
 #define IS_COMMAND() ( \
diff --git a/quantum/keymap_common.c b/quantum/keymap_common.c
index 6cc7adf51fde..746af40c146c 100644
--- a/quantum/keymap_common.c
+++ b/quantum/keymap_common.c
@@ -74,7 +74,21 @@ action_t action_for_key(uint8_t layer, keypos_t key)
     } else if (keycode == RESET) { // RESET is 0x5000, which is why this is here
         clear_keyboard();
         _delay_ms(250);
+// bootloader_jump doesn't work for the Teensy
+#ifdef TEENSY
+        cli();
+        UDCON = 1;
+        USBCON = (1<<FRZCLK);  // disable USB
+        UCSR1B = 0;
+        _delay_ms(5);
+        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");
+#else
         bootloader_jump();
+#endif
         return;
     } else if (keycode == DEBUG) { // DEBUG is 0x5001
       // TODO: Does this actually work?