diff --git a/grbl/grbl.h b/grbl/grbl.h
index 9626365..39fb8e0 100644
--- a/grbl/grbl.h
+++ b/grbl/grbl.h
@@ -39,6 +39,7 @@
 #include <stdbool.h>
 
 // Define the Grbl system include files. NOTE: Do not alter organization.
+#include "debug.h"
 #include "config.h"
 #include "delay.h"
 #include "nuts_bolts.h"
diff --git a/grbl/main.c b/grbl/main.c
index 37d0704..8da129c 100644
--- a/grbl/main.c
+++ b/grbl/main.c
@@ -76,6 +76,7 @@ volatile uint8_t sys_rt_exec_accessory_override; // Global realtime executor bit
 int main(void)
 {
   // Initialize system upon power-up.
+  debug_init();    // Initialize debug leds
   isr_init();      // Set ISR priorities
   delay_init();    // Setup delay timer
   serial_init();   // Setup serial baud rate and interrupts
diff --git a/smoother/debug.h b/smoother/debug.h
new file mode 100644
index 0000000..5d2be06
--- /dev/null
+++ b/smoother/debug.h
@@ -0,0 +1,45 @@
+#pragma once
+
+// Copyright 2017 Todd Fleming
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+// IN THE SOFTWARE.
+
+#include "LPC17xx.h"
+#include <atomic>
+
+static const uint32_t leds = (1 << 21) | (1 << 20) | (1 << 19) | (1 << 18);
+
+inline void debug_init()
+{
+    LPC_GPIO1->FIODIR = leds;
+    LPC_GPIO1->FIOPIN = 0;
+}
+
+inline void debug_enter()
+{
+    LPC_GPIO1->FIOPIN = (LPC_GPIO1->FIOPIN | (1 << 21)) ^ (1 << 20);
+}
+
+inline void debug_exit()
+{
+    LPC_GPIO1->FIOPIN &= ~(1 << 21);
+}
+
+inline void xdebug_enter() {}
+inline void xdebug_exit() {}