diff --git a/core/include/debug.h b/core/include/debug.h
index 06abdbe9f2..2619524a39 100644
--- a/core/include/debug.h
+++ b/core/include/debug.h
@@ -23,12 +23,37 @@
 #define __DEBUG_H
 
 #include <stdio.h>
+#include "sched.h"
+
+#if DEVELHELP
+#include "cpu-conf.h"
+#define DEBUG_PRINT(...) \
+    do { \
+        if ((active_thread == NULL) || (active_thread->stack_size > KERNEL_CONF_STACKSIZE_PRINTF)) { \
+            printf(__VA_ARGS__); \
+        } \
+        else { \
+            puts("Cannot debug, stack too small"); \
+        } \
+    } while (0)
+#else
+#define DEBUG_PRINT(...) printf(__VA_ARGS__)
+#endif
 
 #if ENABLE_DEBUG
-#define DEBUG(...) printf(__VA_ARGS__)
+#include "tcb.h"
+#define DEBUG(...) DEBUG_PRINT(__VA_ARGS__)
+#define DEBUGF(...) \
+    do { \
+        DEBUG_PRINT("DEBUG(%s): %s:%d in %s: ", \
+                active_thread ? active_thread->name : "NO THREAD", \
+                __FILE__, __LINE__, __func__); \
+        DEBUG_PRINT(__VA_ARGS__); \
+    } while (0)
 #undef ENABLE_DEBUG
 #else
 #define DEBUG(...)
+#define DEBUGF(...)
 #endif
 
 /** @} */
diff --git a/core/mutex.c b/core/mutex.c
index ee4b108f13..d2756e4ef3 100644
--- a/core/mutex.c
+++ b/core/mutex.c
@@ -29,6 +29,7 @@
 #include "thread.h"
 #include "irq.h"
 
+#define ENABLE_DEBUG    (0)
 #include "debug.h"
 
 int mutex_init(struct mutex_t *mutex)
diff --git a/examples/rpl_udp/helper.c b/examples/rpl_udp/helper.c
index d5282b948e..1288ca87c7 100644
--- a/examples/rpl_udp/helper.c
+++ b/examples/rpl_udp/helper.c
@@ -29,7 +29,7 @@
 
 #include "demo.h"
 
-#define ENABLE_DEBUG    (1)
+#define ENABLE_DEBUG    (0)
 #include "debug.h"
 
 #define LL_HDR_LEN  (0x4)
@@ -69,7 +69,7 @@ void rpl_udp_monitor(void)
         if (m.type == PKT_PENDING) {
             p = (radio_packet_t *) m.content.ptr;
 
-            DEBUG("Received packet from ID %u\n", p->src);
+            DEBUGF("Received packet from ID %u\n", p->src);
             DEBUG("\tLength:\t%u\n", p->length);
             DEBUG("\tSrc:\t%u\n", p->src);
             DEBUG("\tDst:\t%u\n", p->dst);
diff --git a/examples/rpl_udp/rpl.c b/examples/rpl_udp/rpl.c
index 13f9fdd065..f4d682cd4c 100644
--- a/examples/rpl_udp/rpl.c
+++ b/examples/rpl_udp/rpl.c
@@ -29,6 +29,9 @@
 #include "rpl_dodag.h"
 #include "demo.h"
 
+#define ENABLE_DEBUG    (0)
+#include "debug.h"
+
 #ifdef MODULE_NATIVENET
 #define TRANSCEIVER TRANSCEIVER_NATIVE
 #else
@@ -65,8 +68,10 @@ void rpl_udp_init(int argc, char **argv)
             return;
         }
 
+        DEBUGF("Setting HW address to %u\n", id);
         net_if_set_hardware_address(0, id);
 
+        DEBUGF("Initializing RPL for interface 0\n");
         state = rpl_init(0);
 
         if (state != SIXLOWERROR_SUCCESS) {
@@ -84,7 +89,9 @@ void rpl_udp_init(int argc, char **argv)
             ipv6_iface_set_routing_provider(rpl_get_next_hop);
         }
 
+        DEBUGF("Start monitor\n");
         int monitor_pid = thread_create(monitor_stack_buffer, MONITOR_STACK_SIZE, PRIORITY_MAIN - 2, CREATE_STACKTEST, rpl_udp_monitor, "monitor");
+        DEBUGF("Register at transceiver %02X\n", TRANSCEIVER);
         transceiver_register(TRANSCEIVER, monitor_pid);
         ipv6_register_packet_handler(monitor_pid);
         //sixlowpan_lowpan_register(monitor_pid);