diff --git a/stm/Makefile b/stm/Makefile
index a9cba33bd..6199b89d0 100644
--- a/stm/Makefile
+++ b/stm/Makefile
@@ -32,6 +32,7 @@ SRC_S = \
 
 PY_O = \
 	nlrthumb.o \
+	gc.o \
 	malloc.o \
 	qstr.o \
 	vstr.o \
diff --git a/stm/main.c b/stm/main.c
index 9f7bda8e6..d49c2398b 100644
--- a/stm/main.c
+++ b/stm/main.c
@@ -6,6 +6,8 @@
 #include "std.h"
 
 #include "misc.h"
+#include "mpyconfig.h"
+#include "gc.h"
 #include "systick.h"
 #include "led.h"
 #include "lcd.h"
@@ -14,6 +16,8 @@
 #include "usb.h"
 #include "ff.h"
 
+extern uint32_t _heap_start;
+
 static void impl02_c_version() {
     int x = 0;
     while (x < 400) {
@@ -163,7 +167,6 @@ static void board_info() {
         extern void *_ebss;
         extern void *_estack;
         extern void *_etext;
-        extern void *_heap_start;
         printf("_sidata=%p\n", &_sidata);
         printf("_sdata=%p\n", &_sdata);
         printf("_edata=%p\n", &_edata);
@@ -263,6 +266,14 @@ void do_repl() {
     }
 }
 
+void gc_collect() {
+    gc_collect_start();
+    gc_collect_root((void**)0x20000000, (((uint32_t)&_heap_start) - 0x20000000) / 4);
+    gc_collect_root((void**)(0x20000000 + 0x18000), (0x20000 - 0x18000) / 4);
+    // TODO registers
+    gc_collect_end();
+}
+
 int main() {
     // TODO disable JTAG
 
@@ -284,6 +295,10 @@ int main() {
     lcd_init();
     storage_init();
 
+    // GC init
+    gc_init(&_heap_start, (void*)(0x20000000 + 0x18000));
+    sys_tick_delay_ms(2000);
+
     // Python init
     qstr_init();
     rt_init();
diff --git a/stm/malloc0.c b/stm/malloc0.c
index fc0267911..686dfbf4b 100644
--- a/stm/malloc0.c
+++ b/stm/malloc0.c
@@ -1,6 +1,9 @@
 #include <stdint.h>
 #include "std.h"
+#include "mpyconfig.h"
+#include "gc.h"
 
+#if 0
 static uint32_t mem = 0;
 
 void *malloc(size_t n) {
@@ -20,6 +23,12 @@ void *malloc(size_t n) {
 void free(void *ptr) {
 }
 
+void *realloc(void *ptr, size_t n) {
+    return malloc(n);
+}
+
+#endif
+
 void *calloc(size_t sz, size_t n) {
     char *ptr = malloc(sz * n);
     for (int i = 0; i < sz * n; i++) {
@@ -28,8 +37,15 @@ void *calloc(size_t sz, size_t n) {
     return ptr;
 }
 
+void *malloc(size_t n) {
+    return gc_alloc(n);
+}
+
+void free(void *ptr) {
+}
+
 void *realloc(void *ptr, size_t n) {
-    return malloc(n);
+    return gc_realloc(ptr, n);
 }
 
 void __assert_func() {
diff --git a/stm/mpyconfig.h b/stm/mpyconfig.h
index b6179813d..06a4bd8e0 100644
--- a/stm/mpyconfig.h
+++ b/stm/mpyconfig.h
@@ -8,6 +8,8 @@
 
 // type definitions for the specific machine
 
+#define BYTES_PER_WORD (4)
+
 typedef int32_t machine_int_t; // must be pointer size
 typedef uint32_t machine_uint_t; // must be pointer size
 typedef void *machine_ptr_t; // must be of pointer size
diff --git a/stm/printf.c b/stm/printf.c
index 31ab8c3d2..821b790b4 100644
--- a/stm/printf.c
+++ b/stm/printf.c
@@ -1,5 +1,8 @@
 #include <stdarg.h>
 #include "std.h"
+#include "misc.h"
+#include "lcd.h"
+#include "usb.h"
 
 #define PF_FLAG_LEFT_ADJUST (0x01)
 #define PF_FLAG_SHOW_SIGN   (0x02)
@@ -208,13 +211,13 @@ int pfenv_printf(const pfenv_t *pfenv, const char *fmt, va_list args) {
     return chrs;
 }
 
-void lcd_print_strn(const char *str, unsigned int len);
-void usb_vcp_send_strn(const char* str, int len);
-
 void stdout_print_strn(void *data, const char *str, unsigned int len) {
     // send stdout to LCD and USB CDC VCP
-    lcd_print_strn(str, len);
-    usb_vcp_send_strn(str, len);
+    if (usb_vcp_is_enabled()) {
+        usb_vcp_send_strn(str, len);
+    } else {
+        lcd_print_strn(str, len);
+    }
 }
 
 static const pfenv_t pfenv_stdout = {0, stdout_print_strn};
diff --git a/stm/usb.c b/stm/usb.c
index 0b88e7bf7..4e8b454f1 100644
--- a/stm/usb.c
+++ b/stm/usb.c
@@ -12,11 +12,12 @@
 
 extern CDC_IF_Prop_TypeDef VCP_fops;
 
-int is_enabled = 0;
 USB_OTG_CORE_HANDLE USB_OTG_dev;
-char rx_buf[64];
-int rx_buf_in;
-int rx_buf_out;
+
+static int is_enabled = 0;
+static char rx_buf[64];
+static int rx_buf_in;
+static int rx_buf_out;
 
 void usb_init() {
     USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_PYB_cb, &USR_cb);
@@ -25,6 +26,10 @@ void usb_init() {
     is_enabled = 1;
 }
 
+bool usb_vcp_is_enabled() {
+    return is_enabled;
+}
+
 void usb_vcp_receive(const char *buf, uint32_t len) {
     if (is_enabled) {
         for (int i = 0; i < len; i++) {
diff --git a/stm/usb.h b/stm/usb.h
index 14a0345c1..75b7bb346 100644
--- a/stm/usb.h
+++ b/stm/usb.h
@@ -1,4 +1,5 @@
 void usb_init();
+bool usb_vcp_is_enabled();
 int usb_vcp_rx_any();
 char usb_vcp_rx_get();
 void usb_vcp_send_str(const char* str);