5 files changed, 53 insertions, 8 deletions
diff --git a/teensy/main.c b/teensy/main.c
index 66f3d935f..4fa7aaab3 100644
--- a/teensy/main.c
+++ b/teensy/main.c
@@ -18,11 +18,9 @@
 #include MICROPY_HAL_H
 
 #include "servo.h"
-#include "usb.h"
 #include "led.h"
 #include "uart.h"
 #include "pin.h"
-#include "pybstdio.h"
 
 
 extern uint32_t _heap_start;
@@ -43,8 +41,8 @@ void NORETURN __fatal_error(const char *msg) {
     led_state(2, 1);
     led_state(3, 1);
     led_state(4, 1);
-    stdout_tx_strn("\nFATAL ERROR:\n", 14);
-    stdout_tx_strn(msg, strlen(msg));
+    mp_hal_stdout_tx_strn("\nFATAL ERROR:\n", 14);
+    mp_hal_stdout_tx_strn(msg, strlen(msg));
     for (uint i = 0;;) {
         led_toggle(((i++) & 3) + 1);
         for (volatile uint delay = 0; delay < 10000000; delay++) {
diff --git a/teensy/modpyb.c b/teensy/modpyb.c
index 13454b253..a59e3b4f5 100644
--- a/teensy/modpyb.c
+++ b/teensy/modpyb.c
@@ -56,7 +56,6 @@
 #include "dac.h"
 #include "usb.h"
 #include "portmodules.h"
-#include "pybstdio.h"
 
 /// \module pyb - functions related to the pyboard
 ///
diff --git a/teensy/teensy_hal.c b/teensy/teensy_hal.c
index c28dd9182..312aca058 100644
--- a/teensy/teensy_hal.c
+++ b/teensy/teensy_hal.c
@@ -1,9 +1,10 @@
 #include <stdio.h>
+#include <string.h>
 
-#include "py/mpconfig.h"
-
+#include "py/mpstate.h"
+#include "usb.h"
+#include "uart.h"
 #include "Arduino.h"
-
 #include MICROPY_HAL_H
 
 uint32_t HAL_GetTick(void) {
@@ -19,3 +20,38 @@ void mp_hal_set_interrupt_char(int c) {
   // an exception when a certain character is received. That just means that
   // you can't press Control-C and get your python script to stop.
 }
+
+int mp_hal_stdin_rx_chr(void) {
+    for (;;) {
+        byte c;
+        if (usb_vcp_recv_byte(&c) != 0) {
+            return c;
+        } else if (MP_STATE_PORT(pyb_stdio_uart) != NULL && uart_rx_any(MP_STATE_PORT(pyb_stdio_uart))) {
+            return uart_rx_char(MP_STATE_PORT(pyb_stdio_uart));
+        }
+        __WFI();
+    }
+}
+
+void mp_hal_stdout_tx_str(const char *str) {
+    mp_hal_stdout_tx_strn(str, strlen(str));
+}
+
+void mp_hal_stdout_tx_strn(const char *str, uint32_t len) {
+    if (MP_STATE_PORT(pyb_stdio_uart) != NULL) {
+        uart_tx_strn(MP_STATE_PORT(pyb_stdio_uart), str, len);
+    }
+    if (usb_vcp_is_enabled()) {
+        usb_vcp_send_strn(str, len);
+    }
+}
+
+void mp_hal_stdout_tx_strn_cooked(const char *str, uint32_t len) {
+    // send stdout to UART and USB CDC VCP
+    if (MP_STATE_PORT(pyb_stdio_uart) != NULL) {
+        uart_tx_strn_cooked(MP_STATE_PORT(pyb_stdio_uart), str, len);
+    }
+    if (usb_vcp_is_enabled()) {
+        usb_vcp_send_strn_cooked(str, len);
+    }
+}
diff --git a/teensy/teensy_hal.h b/teensy/teensy_hal.h
index ee434fd20..f5c6d18d7 100644
--- a/teensy/teensy_hal.h
+++ b/teensy/teensy_hal.h
@@ -116,6 +116,11 @@ uint32_t HAL_GetTick(void);
 void     HAL_Delay(uint32_t Delay);
 void mp_hal_set_interrupt_char(int c);
 
+int mp_hal_stdin_rx_chr(void);
+void mp_hal_stdout_tx_str(const char *str);
+void mp_hal_stdout_tx_strn(const char *str, uint32_t len);
+void mp_hal_stdout_tx_strn_cooked(const char *str, uint32_t len);
+
 void HAL_GPIO_Init(GPIO_TypeDef *GPIOx, GPIO_InitTypeDef *init);
 
 #define GPIO_read_pin(gpio, pin)        (((gpio)->PDIR >> (pin)) & 1)
diff --git a/teensy/usb.h b/teensy/usb.h
new file mode 100644
index 000000000..949d7a59c
--- /dev/null
+++ b/teensy/usb.h
@@ -0,0 +1,7 @@
+bool usb_vcp_is_connected(void);
+bool usb_vcp_is_enabled(void);
+int usb_vcp_rx_num(void);
+int usb_vcp_recv_byte(uint8_t *ptr);
+void usb_vcp_send_str(const char* str);
+void usb_vcp_send_strn(const char* str, int len);
+void usb_vcp_send_strn_cooked(const char *str, int len);