diff --git a/main.c b/main.c
old mode 100644
new mode 100755
index cbc0b093c3..91d8200892
--- a/main.c
+++ b/main.c
@@ -44,6 +44,7 @@
 #include "lib/utils/pyexec.h"
 
 #include "mpconfigboard.h"
+#include "supervisor/cpu.h"
 #include "supervisor/port.h"
 #include "supervisor/filesystem.h"
 // TODO(tannewt): Figure out how to choose language at compile time.
@@ -381,16 +382,17 @@ int __attribute__((used)) main(void) {
 }
 
 void gc_collect(void) {
-    // WARNING: This gc_collect implementation doesn't try to get root
-    // pointers from CPU registers, and thus may function incorrectly.
-    void *dummy;
     gc_collect_start();
+
+    mp_uint_t regs[10];
+    mp_uint_t sp = cpu_get_regs_and_sp(regs);
+
     // This collects root pointers from the VFS mount table. Some of them may
     // have lost their references in the VM even though they are mounted.
     gc_collect_root((void**)&MP_STATE_VM(vfs_mount_table), sizeof(mp_vfs_mount_t) / sizeof(mp_uint_t));
     // This naively collects all object references from an approximate stack
     // range.
-    gc_collect_root(&dummy, ((mp_uint_t)&_estack - (mp_uint_t)&dummy) / sizeof(mp_uint_t));
+    gc_collect_root((void**)sp, ((uint32_t)&_estack - sp) / sizeof(uint32_t));
     gc_collect_end();
 }
 
diff --git a/ports/atmel-samd/Makefile b/ports/atmel-samd/Makefile
old mode 100644
new mode 100755
index 731e826776..9a40102700
--- a/ports/atmel-samd/Makefile
+++ b/ports/atmel-samd/Makefile
@@ -412,6 +412,8 @@ endif
 SRC_SHARED_MODULE_EXPANDED = $(addprefix shared-bindings/, $(SRC_SHARED_MODULE)) \
                              $(addprefix shared-module/, $(SRC_SHARED_MODULE))
 
+SRC_O = supervisor/$(CHIP_FAMILY)_cpu.o
+
 OBJ = $(PY_O) $(SUPERVISOR_O) $(addprefix $(BUILD)/, $(SRC_C:.c=.o))
 OBJ += $(addprefix $(BUILD)/, $(SRC_ASF:.c=.o))
 OBJ += $(addprefix $(BUILD)/, $(SRC_COMMON_HAL_EXPANDED:.c=.o))
@@ -419,6 +421,7 @@ OBJ += $(addprefix $(BUILD)/, $(SRC_SHARED_MODULE_EXPANDED:.c=.o))
 ifeq ($(INTERNAL_LIBM),1)
 OBJ += $(addprefix $(BUILD)/, $(SRC_LIBM:.c=.o))
 endif
+OBJ += $(addprefix $(BUILD)/, $(SRC_O))
 
 SRC_QSTR += $(SRC_C) $(SRC_SUPERVISOR) $(SRC_COMMON_HAL_EXPANDED) $(SRC_SHARED_MODULE_EXPANDED) $(STM_SRC_C)
 
diff --git a/ports/atmel-samd/supervisor/samd21_cpu.s b/ports/atmel-samd/supervisor/samd21_cpu.s
new file mode 100755
index 0000000000..741bb21358
--- /dev/null
+++ b/ports/atmel-samd/supervisor/samd21_cpu.s
@@ -0,0 +1,35 @@
+.syntax unified
+.cpu cortex-m0
+.thumb
+.text
+.align  2
+
+@ uint cpu_get_regs_and_sp(r0=uint regs[10])
+.global cpu_get_regs_and_sp
+.thumb
+.thumb_func
+.type   cpu_get_regs_and_sp, %function
+cpu_get_regs_and_sp:
+@ store registers into given array
+str     r4, [r0, #0]
+str     r5, [r0, #4]
+str     r6, [r0, #8]
+str     r7, [r0, #12]
+push {r1}
+mov     r1, r8
+str     r1, [r0, #16]
+mov     r1, r9
+str     r1, [r0, #20]
+mov     r1, r10
+str     r1, [r0, #24]
+mov     r1, r11
+str     r1, [r0, #28]
+mov     r1, r12
+str     r1, [r0, #32]
+mov     r1, r13
+str     r1, [r0, #36]
+pop {r1}
+
+@ return the sp
+mov     r0, sp
+bx      lr
diff --git a/ports/atmel-samd/supervisor/samd51_cpu.s b/ports/atmel-samd/supervisor/samd51_cpu.s
new file mode 100755
index 0000000000..9e6807a5e2
--- /dev/null
+++ b/ports/atmel-samd/supervisor/samd51_cpu.s
@@ -0,0 +1,27 @@
+.syntax unified
+.cpu cortex-m4
+.thumb
+.text
+.align  2
+
+@ uint cpu_get_regs_and_sp(r0=uint regs[10])
+.global cpu_get_regs_and_sp
+.thumb
+.thumb_func
+.type   cpu_get_regs_and_sp, %function
+cpu_get_regs_and_sp:
+@ store registers into given array
+str     r4, [r0], #4
+str     r5, [r0], #4
+str     r6, [r0], #4
+str     r7, [r0], #4
+str     r8, [r0], #4
+str     r9, [r0], #4
+str     r10, [r0], #4
+str     r11, [r0], #4
+str     r12, [r0], #4
+str     r13, [r0], #4
+
+@ return the sp
+mov     r0, sp
+bx      lr
diff --git a/supervisor/cpu.h b/supervisor/cpu.h
new file mode 100755
index 0000000000..c4f81316c7
--- /dev/null
+++ b/supervisor/cpu.h
@@ -0,0 +1,34 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2017 Scott Shawcroft for Adafruit Industries
+ *
+ * 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.
+ */
+
+#ifndef MICROPY_INCLUDED_SUPERVISOR_CPU_H
+#define MICROPY_INCLUDED_SUPERVISOR_CPU_H
+
+// Adds up to 10 pointers from the CPUs registers to regs. This is used to make sure no actively
+// used heap memory is freed. Its usually implemented in assembly.
+mp_uint_t cpu_get_regs_and_sp(mp_uint_t *regs);
+
+#endif  // MICROPY_INCLUDED_SUPERVISOR_CPU_H