Updated teensy to work with latest on master

Added analogRead, analogWriteXxx and servo support for teensy.

9 files changed, 447 insertions, 64 deletions

diff --git a/teensy/Makefile b/teensy/Makefile
index 0cd7272e71..5630103e34 100644
--- a/teensy/Makefile
+++ b/teensy/Makefile
@@ -1,5 +1,8 @@
 include ../py/mkenv.mk
 
+# qstr definitions (must come before including py.mk)
+QSTR_DEFS = qstrdefsport.h
+
 # include py core make definitions
 include ../py/py.mk
 
@@ -32,6 +35,8 @@ SRC_C = \
 	lexerfatfs.c \
 	lexermemzip.c \
 	memzip.c \
+	servo.c \
+	usart.c \
 	usb.c \
 
 STM_SRC_C = $(addprefix stm/,\
@@ -54,7 +59,7 @@ SRC_TEENSY = \
 	usb_serial.c \
 	yield.c \
 
-OBJ = $(addprefix $(BUILD)/, $(SRC_C:.c=.o) $(STM_SRC_C:.c=.o) $(STM_SRC_S:.s=.o) $(SRC_TEENSY:.c=.o)) $(PY_O)
+OBJ = $(PY_O) $(addprefix $(BUILD)/, $(SRC_C:.c=.o) $(STM_SRC_C:.c=.o) $(STM_SRC_S:.s=.o) $(SRC_TEENSY:.c=.o))
 #LIB = -lreadline
 # the following is needed for BSD
 #LIB += -ltermcap
diff --git a/teensy/led.c b/teensy/led.c
index 0cef354c27..5eace0d373 100644
--- a/teensy/led.c
+++ b/teensy/led.c
@@ -2,6 +2,7 @@
 
 #include "misc.h"
 #include "mpconfig.h"
+#include "qstr.h"
 #include "obj.h"
 #include "led.h"
 
diff --git a/teensy/lexerfatfs.c b/teensy/lexerfatfs.c
index 2ded0756f7..95084b6501 100644
--- a/teensy/lexerfatfs.c
+++ b/teensy/lexerfatfs.c
@@ -2,11 +2,13 @@
 #include <stdio.h>
 
 #include "misc.h"
+#include "mpconfig.h"
+#include "qstr.h"
 #include "lexer.h"
 typedef int FIL;
 #include "../stm/lexerfatfs.h"
 
-mp_lexer_t *mp_lexer_new_from_file(const char *filename, mp_lexer_file_buf_t *fb) {
+mp_lexer_t *mp_lexer_new_from_file(const char *filename) {
   printf("import not implemented\n");
   return NULL;
 }
@@ -15,3 +17,8 @@ mp_lexer_t *mp_import_open_file(qstr mod_name) {
     printf("import not implemented\n");
     return NULL;
 }
+
+mp_import_stat_t mp_import_stat(const char *path) {
+    // TODO implement me!
+    return MP_IMPORT_STAT_NO_EXIST;
+}
diff --git a/teensy/lexermemzip.c b/teensy/lexermemzip.c
index 2f808ee429..3e15717acf 100644
--- a/teensy/lexermemzip.c
+++ b/teensy/lexermemzip.c
@@ -2,6 +2,8 @@
 #include <stdlib.h>
 
 #include "misc.h"
+#include "mpconfig.h"
+#include "qstr.h"
 #include "lexer.h"
 #include "memzip.h"
 
@@ -13,6 +15,7 @@ mp_lexer_t *mp_lexer_new_from_memzip_file(const char *filename)
     if (memzip_locate(filename, &data, &len) != MZ_OK) {
         return NULL;
     }
-    return mp_lexer_new_from_str_len(filename, (const char *)data, (uint)len, 0);
+
+    return mp_lexer_new_from_str_len(qstr_from_str(filename), (const char *)data, (uint)len, 0);
 }
 
diff --git a/teensy/main.c b/teensy/main.c
index 0bc085aed9..bbfa7590fa 100644
--- a/teensy/main.c
+++ b/teensy/main.c
@@ -6,7 +6,7 @@
 #include "nlr.h"
 #include "misc.h"
 #include "mpconfig.h"
-#include "mpqstr.h"
+#include "qstr.h"
 #include "lexer.h"
 #include "lexermemzip.h"
 #include "parse.h"
@@ -15,6 +15,7 @@
 #include "runtime0.h"
 #include "runtime.h"
 #include "repl.h"
+#include "servo.h"
 #include "usb.h"
 #include "gc.h"
 #include "led.h"
@@ -54,6 +55,32 @@ static const char *help_text =
 #endif
 ;
 
+mp_obj_t pyb_analog_read(mp_obj_t pin_obj) {
+    uint pin = mp_obj_get_int(pin_obj);
+    int val = analogRead(pin);
+    return MP_OBJ_NEW_SMALL_INT(val);
+}
+
+mp_obj_t pyb_analog_write(mp_obj_t pin_obj, mp_obj_t val_obj) {
+    uint pin = mp_obj_get_int(pin_obj);
+    int val = mp_obj_get_int(val_obj);
+    analogWrite(pin, val);
+    return mp_const_none;
+}
+
+mp_obj_t pyb_analog_write_resolution(mp_obj_t res_obj) {
+    int res = mp_obj_get_int(res_obj);
+    analogWriteResolution(res);
+    return mp_const_none;
+}
+
+mp_obj_t pyb_analog_write_frequency(mp_obj_t pin_obj, mp_obj_t freq_obj) {
+    uint pin = mp_obj_get_int(pin_obj);
+    int freq = mp_obj_get_int(freq_obj);
+    analogWriteFrequency(pin, freq);
+    return mp_const_none;
+}
+
 // get some help about available functions
 static mp_obj_t pyb_help(void) {
     printf("%s", help_text);
@@ -181,16 +208,26 @@ mp_obj_t pyb_hid_send_report(mp_obj_t arg) {
 }
 #endif
 
-static qstr pyb_config_source_dir = 0;
-static qstr pyb_config_main = 0;
+static mp_obj_t pyb_config_source_dir = MP_OBJ_NULL;
+static mp_obj_t pyb_config_main = MP_OBJ_NULL;
 
 mp_obj_t pyb_source_dir(mp_obj_t source_dir) {
-    pyb_config_source_dir = mp_obj_get_qstr(source_dir);
+    if (MP_OBJ_IS_STR(source_dir)) {
+        pyb_config_source_dir = source_dir;
+        printf("source_dir = '");
+        mp_obj_print(source_dir, PRINT_STR);
+        printf("'\n");
+    }
     return mp_const_none;
 }
 
 mp_obj_t pyb_main(mp_obj_t main) {
-    pyb_config_main = mp_obj_get_qstr(main);
+    if (MP_OBJ_IS_STR(main)) {
+        pyb_config_main = main;
+        printf("main = '");
+        mp_obj_print(main, PRINT_STR);
+        printf("'\n");
+    }
     return mp_const_none;
 }
 
@@ -205,7 +242,7 @@ mp_obj_t pyb_led(mp_obj_t state) {
 }
 
 mp_obj_t pyb_run(mp_obj_t filename_obj) {
-    const char *filename = qstr_str(mp_obj_get_qstr(filename_obj));
+    const char *filename = qstr_str(mp_obj_str_get_qstr(filename_obj));
     do_file(filename);
     return mp_const_none;
 }
@@ -309,15 +346,24 @@ bool do_file(const char *filename) {
         return false;
     }
 
-    mp_parse_node_t pn = mp_parse(lex, MP_PARSE_FILE_INPUT);
+    qstr parse_exc_id;
+    const char *parse_exc_msg;
+    mp_parse_node_t pn = mp_parse(lex, MP_PARSE_FILE_INPUT, &parse_exc_id, &parse_exc_msg);
     qstr source_name = mp_lexer_source_name(lex);
-    mp_lexer_free(lex);
 
     if (pn == MP_PARSE_NODE_NULL) {
+        // parse error
+        mp_lexer_show_error_pythonic_prefix(lex);
+        printf("%s: %s\n", qstr_str(parse_exc_id), parse_exc_msg);
+        mp_lexer_free(lex);
         return false;
     }
 
+    mp_lexer_free(lex);
+
     mp_obj_t module_fun = mp_compile(pn, source_name, false);
+    mp_parse_node_free(pn);
+
     if (module_fun == mp_const_none) {
         return false;
     }
@@ -329,7 +375,7 @@ bool do_file(const char *filename) {
         return true;
     } else {
         // uncaught exception
-        mp_obj_print((mp_obj_t)nlr.ret_val);
+        mp_obj_print((mp_obj_t)nlr.ret_val, PRINT_REPR);
         printf("\n");
         return false;
     }
@@ -340,7 +386,7 @@ void do_repl(void) {
     stdout_tx_str("Type \"help()\" for more information.\r\n");
 
     vstr_t line;
-    vstr_init(&line);
+    vstr_init(&line, 32);
 
     for (;;) {
         vstr_reset(&line);
@@ -366,12 +412,21 @@ void do_repl(void) {
             }
         }
 
-        mp_lexer_t *lex = mp_lexer_new_from_str_len("<stdin>", vstr_str(&line), vstr_len(&line), 0);
-        mp_parse_node_t pn = mp_parse(lex, MP_PARSE_SINGLE_INPUT);
-        mp_lexer_free(lex);
-
-        if (pn != MP_PARSE_NODE_NULL) {
-            mp_obj_t module_fun = mp_compile(pn, true);
+        mp_lexer_t *lex = mp_lexer_new_from_str_len(MP_QSTR__lt_stdin_gt_, vstr_str(&line), vstr_len(&line), 0);
+        qstr parse_exc_id;
+        const char *parse_exc_msg;
+        mp_parse_node_t pn = mp_parse(lex, MP_PARSE_SINGLE_INPUT, &parse_exc_id, &parse_exc_msg);
+        qstr source_name = mp_lexer_source_name(lex);
+
+        if (pn == MP_PARSE_NODE_NULL) {
+            // parse error
+            mp_lexer_show_error_pythonic_prefix(lex);
+            printf("%s: %s\n", qstr_str(parse_exc_id), parse_exc_msg);
+            mp_lexer_free(lex);
+        } else {
+            // parse okay
+            mp_lexer_free(lex);
+            mp_obj_t module_fun = mp_compile(pn, source_name, true);
             if (module_fun != mp_const_none) {
                 nlr_buf_t nlr;
                 uint32_t start = micros();
@@ -381,11 +436,11 @@ void do_repl(void) {
                     // optional timing
                     if (0) {
                         uint32_t ticks = micros() - start; // TODO implement a function that does this properly
-                        printf("(took %lu us)\n", ticks);
+                        printf("(took %lu ms)\n", ticks);
                     }
                 } else {
                     // uncaught exception
-                    mp_obj_print((mp_obj_t)nlr.ret_val);
+                    mp_obj_print((mp_obj_t)nlr.ret_val, PRINT_REPR);
                     printf("\n");
                 }
             }
@@ -396,11 +451,15 @@ void do_repl(void) {
 }
 
 int main(void) {
-    pinMode(LED_BUILTIN, OUTPUT);    
+    pinMode(LED_BUILTIN, OUTPUT);
+#if 0
     // Wait for host side to get connected
     while (!usb_vcp_is_connected()) {
         ;
     }
+#else
+    delay(1000);
+#endif
 
     led_init();
     led_state(PYB_LED_BUILTIN, 1);
@@ -415,28 +474,34 @@ soft_reset:
     qstr_init();
     rt_init();
 
-#if 1
     // add some functions to the python namespace
     {
-        rt_store_name(qstr_from_str_static("help"), rt_make_function_0(pyb_help));
-        mp_obj_t m = mp_obj_new_module(qstr_from_str_static("pyb"));
-        rt_store_attr(m, qstr_from_str_static("info"), rt_make_function_0(pyb_info));
-        rt_store_attr(m, qstr_from_str_static("source_dir"), rt_make_function_1(pyb_source_dir));
-        rt_store_attr(m, qstr_from_str_static("main"), rt_make_function_1(pyb_main));
-        rt_store_attr(m, qstr_from_str_static("gc"), rt_make_function_0(pyb_gc));
-        rt_store_attr(m, qstr_from_str_static("delay"), rt_make_function_1(pyb_delay));
-        rt_store_attr(m, qstr_from_str_static("led"), rt_make_function_1(pyb_led));
-        rt_store_attr(m, qstr_from_str_static("Led"), rt_make_function_1(pyb_Led));
-        rt_store_attr(m, qstr_from_str_static("gpio"), (mp_obj_t)&pyb_gpio_obj);
-        rt_store_name(qstr_from_str_static("pyb"), m);
-        rt_store_name(qstr_from_str_static("run"), rt_make_function_1(pyb_run));
+        rt_store_name(MP_QSTR_help, rt_make_function_n(0, pyb_help));
+        mp_obj_t m = mp_obj_new_module(MP_QSTR_pyb);
+        rt_store_attr(m, MP_QSTR_info, rt_make_function_n(0, pyb_info));
+        rt_store_attr(m, MP_QSTR_source_dir, rt_make_function_n(1, pyb_source_dir));
+        rt_store_attr(m, MP_QSTR_main, rt_make_function_n(1, pyb_main));
+        rt_store_attr(m, MP_QSTR_gc, rt_make_function_n(0, pyb_gc));
+        rt_store_attr(m, MP_QSTR_delay, rt_make_function_n(1, pyb_delay));
+        rt_store_attr(m, MP_QSTR_led, rt_make_function_n(1, pyb_led));
+        rt_store_attr(m, MP_QSTR_Led, rt_make_function_n(1, pyb_Led));
+        rt_store_attr(m, MP_QSTR_analogRead, rt_make_function_n(1, pyb_analog_read));
+        rt_store_attr(m, MP_QSTR_analogWrite, rt_make_function_n(2, pyb_analog_write));
+        rt_store_attr(m, MP_QSTR_analogWriteResolution, rt_make_function_n(1, pyb_analog_write_resolution));
+        rt_store_attr(m, MP_QSTR_analogWriteFrequency, rt_make_function_n(2, pyb_analog_write_frequency));
+
+        rt_store_attr(m, MP_QSTR_gpio, (mp_obj_t)&pyb_gpio_obj);
+        rt_store_attr(m, MP_QSTR_Servo, rt_make_function_n(0, pyb_Servo));
+        rt_store_name(MP_QSTR_pyb, m);
+        rt_store_name(MP_QSTR_run, rt_make_function_n(1, pyb_run));
     }
-#endif
 
+    printf("About execute /boot.py\n");
     if (!do_file("/boot.py")) {
         printf("Unable to open '/boot.py'\n");
         flash_error(4);
     }
+    printf("Done executing /boot.py\n");
 
     // Turn bootup LED off
     led_state(PYB_LED_BUILTIN, 0);
@@ -445,21 +510,23 @@ soft_reset:
     {
         vstr_t *vstr = vstr_new();
         vstr_add_str(vstr, "/");
-        if (pyb_config_source_dir == 0) {
+        if (pyb_config_source_dir == MP_OBJ_NULL) {
             vstr_add_str(vstr, "src");
         } else {
-            vstr_add_str(vstr, qstr_str(pyb_config_source_dir));
+            vstr_add_str(vstr, mp_obj_str_get_str(pyb_config_source_dir));
         }
         vstr_add_char(vstr, '/');
-        if (pyb_config_main == 0) {
+        if (pyb_config_main == MP_OBJ_NULL) {
             vstr_add_str(vstr, "main.py");
         } else {
-            vstr_add_str(vstr, qstr_str(pyb_config_main));
+            vstr_add_str(vstr, mp_obj_str_get_str(pyb_config_main));
         }
+        printf("About execute '%s'\n", vstr_str(vstr));
         if (!do_file(vstr_str(vstr))) {
             printf("Unable to open '%s'\n", vstr_str(vstr));
             flash_error(3);
         }
+        printf("Done executing '%s'\n", vstr_str(vstr));
         vstr_free(vstr);
     }
 
@@ -471,16 +538,6 @@ soft_reset:
     goto soft_reset;
 }
 
-double __aeabi_f2d(float x) {
-    // TODO
-    return 0.0;
-}
-
-float __aeabi_d2f(double x) {
-    // TODO
-    return 0.0;
-}
-
 double sqrt(double x) {
     // TODO
     return 0.0;
diff --git a/teensy/qstrdefsport.h b/teensy/qstrdefsport.h
new file mode 100644
index 0000000000..8b23a86bf0
--- /dev/null
+++ b/teensy/qstrdefsport.h
@@ -0,0 +1,36 @@
+// qstrs specific to this port
+
+Q(help)
+Q(pyb)
+Q(info)
+Q(sd_test)
+Q(stop)
+Q(standby)
+Q(source_dir)
+Q(main)
+Q(sync)
+Q(gc)
+Q(delay)
+Q(switch)
+Q(servo)
+Q(pwm)
+Q(accel)
+Q(mma_read)
+Q(mma_mode)
+Q(hid)
+Q(time)
+Q(rand)
+Q(Led)
+Q(led)
+Q(Servo)
+Q(I2C)
+Q(gpio)
+Q(Usart)
+Q(ADC)
+Q(open)
+Q(analogRead)
+Q(analogWrite)
+Q(analogWriteResolution)
+Q(analogWriteFrequency)
+Q(run)
+
diff --git a/teensy/servo.c b/teensy/servo.c
new file mode 100644
index 0000000000..3d6c9420d2
--- /dev/null
+++ b/teensy/servo.c
@@ -0,0 +1,264 @@
+#include <stdio.h>
+#include "misc.h"
+#include "mpconfig.h"
+#include "qstr.h"
+#include "nlr.h"
+#include "obj.h"
+#include "servo.h"
+
+#include "Arduino.h"
+
+#define MAX_SERVOS  12
+#define INVALID_SERVO   -1
+
+#define MIN_PULSE_WIDTH       544     // the shortest pulse sent to a servo
+#define MAX_PULSE_WIDTH      2400     // the longest pulse sent to a servo
+#define DEFAULT_PULSE_WIDTH  1500     // default pulse width when servo is attached
+#define REFRESH_INTERVAL    20000     // minumim time to refresh servos in microseconds
+
+#define PDB_CONFIG (PDB_SC_TRGSEL(15) | PDB_SC_PDBEN | PDB_SC_PDBIE \
+	| PDB_SC_CONT | PDB_SC_PRESCALER(2) | PDB_SC_MULT(0))
+#define PDB_PRESCALE 4
+#define usToTicks(us)    ((us) * (F_BUS / 1000) / PDB_PRESCALE / 1000)
+#define ticksToUs(ticks) ((ticks) * PDB_PRESCALE * 1000 / (F_BUS / 1000))
+
+static uint16_t servo_active_mask = 0;
+static uint16_t servo_allocated_mask = 0;
+static uint8_t servo_pin[MAX_SERVOS];
+static uint16_t servo_ticks[MAX_SERVOS];
+
+typedef struct _pyb_servo_obj_t {
+    mp_obj_base_t base;
+    uint servo_id;
+    uint min_usecs;
+    uint max_usecs;
+} pyb_servo_obj_t;
+
+#define clamp(v, min_val, max_val) ((v) < (min_val) ? (min_val) : (v) > (max_val) ? (max_val) : (v))
+
+static float map_uint_to_float(uint x, uint in_min, uint in_max, float out_min, float out_max)
+{
+	return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + (float)out_min;
+}
+
+static uint map_float_to_uint(float x, float in_min, float in_max, uint out_min, uint out_max)
+{
+	return (int)((x - in_min) * (float)(out_max - out_min) / (in_max - in_min) + (float)out_min);
+}
+
+static mp_obj_t servo_obj_attach(mp_obj_t self_in, mp_obj_t pin_obj) {
+    pyb_servo_obj_t *self = self_in;
+    uint pin = mp_obj_get_int(pin_obj);
+    if (pin > CORE_NUM_DIGITAL) {
+        goto pin_error;
+    }
+
+    pinMode(pin, OUTPUT);
+    servo_pin[self->servo_id] = pin;
+    servo_active_mask |= (1 << self->servo_id);
+    if (!(SIM_SCGC6 & SIM_SCGC6_PDB)) {
+        SIM_SCGC6 |= SIM_SCGC6_PDB; // TODO: use bitband for atomic bitset
+        PDB0_MOD = 0xFFFF;
+        PDB0_CNT = 0;
+        PDB0_IDLY = 0;
+        PDB0_SC = PDB_CONFIG;
+        // TODO: maybe this should be a higher priority than most
+        // other interrupts (init all to some default?)
+        PDB0_SC = PDB_CONFIG | PDB_SC_SWTRIG;
+    }
+    NVIC_ENABLE_IRQ(IRQ_PDB);
+    return mp_const_none;
+
+pin_error:
+    nlr_jump(mp_obj_new_exception_msg_varg(MP_QSTR_ValueError, "pin %d does not exist", pin));
+}
+
+static mp_obj_t servo_obj_detach(mp_obj_t self_in) {
+    //pyb_servo_obj_t *self = self_in;
+    return mp_const_none;
+}
+
+static mp_obj_t servo_obj_pin(mp_obj_t self_in) {
+    pyb_servo_obj_t *self = self_in;
+    return MP_OBJ_NEW_SMALL_INT(servo_pin[self->servo_id]);
+}
+
+static mp_obj_t servo_obj_min_usecs(int n_args, const mp_obj_t *args) {
+    pyb_servo_obj_t *self = args[0];
+    if (n_args == 1) {
+        // get min
+        return MP_OBJ_NEW_SMALL_INT(self->min_usecs);
+    }
+    // Set min
+    self->min_usecs = mp_obj_get_int(args[1]);
+    return mp_const_none;
+}
+
+static mp_obj_t servo_obj_max_usecs(int n_args, const mp_obj_t *args) {
+    pyb_servo_obj_t *self = args[0];
+    if (n_args == 1) {
+        // get max
+        return MP_OBJ_NEW_SMALL_INT(self->max_usecs);
+    }
+    // Set max
+    self->max_usecs = mp_obj_get_int(args[1]);
+    return mp_const_none;
+}
+
+static mp_obj_t servo_obj_angle(int n_args, const mp_obj_t *args) {
+    pyb_servo_obj_t *self = args[0];
+    if (n_args == 1) {
+        // get
+        float angle = map_uint_to_float(servo_ticks[self->servo_id],
+                                        usToTicks(self->min_usecs),
+                                        usToTicks(self->max_usecs),
+                                        0.0, 180.0);
+        return mp_obj_new_float(angle);
+    }
+    // Set
+    float angle = mp_obj_get_float(args[1]);
+    if (angle < 0.0F) {
+        angle = 0.0F;
+    }
+    if (angle > 180.0F) {
+        angle = 180.0F;
+    }
+    servo_ticks[self->servo_id] = map_float_to_uint(angle,
+                                                    0.0F, 180.0F,
+                                                    usToTicks(self->min_usecs),
+                                                    usToTicks(self->max_usecs));
+    return mp_const_none;
+}
+
+static mp_obj_t servo_obj_usecs(int n_args, const mp_obj_t *args) {
+    pyb_servo_obj_t *self = args[0];
+    uint usecs;
+    if (n_args == 1) {
+        // get
+        return MP_OBJ_NEW_SMALL_INT(ticksToUs(servo_ticks[self->servo_id]));
+    }
+    // Set
+    usecs = mp_obj_get_int(args[1]);
+
+    if (self->min_usecs < self->max_usecs) {
+        usecs = clamp(usecs, self->min_usecs, self->max_usecs);
+    } else {
+        usecs = clamp(usecs, self->max_usecs, self->min_usecs);
+    }
+    servo_ticks[self->servo_id] = usToTicks(usecs);
+    return mp_const_none;
+}
+
+static mp_obj_t servo_obj_attached(mp_obj_t self_in) {
+    pyb_servo_obj_t *self = self_in;
+    uint attached = (servo_active_mask & (1 << self->servo_id)) != 0;
+    return MP_OBJ_NEW_SMALL_INT(attached);
+}
+
+static void servo_obj_print(void (*print)(void *env, const char *fmt, ...), void *env, mp_obj_t self_in) {
+    pyb_servo_obj_t *self = self_in;
+    print(env, "<Servo %lu>", self->servo_id);
+}
+
+static MP_DEFINE_CONST_FUN_OBJ_2(servo_obj_attach_obj, servo_obj_attach);
+static MP_DEFINE_CONST_FUN_OBJ_1(servo_obj_detach_obj, servo_obj_detach);
+static MP_DEFINE_CONST_FUN_OBJ_1(servo_obj_pin_obj, servo_obj_pin);
+static MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(servo_obj_min_usecs_obj, 1, 2, servo_obj_min_usecs);
+static MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(servo_obj_max_usecs_obj, 1, 2, servo_obj_max_usecs);
+static MP_DEFINE_CONST_FUN_OBJ_1(servo_obj_attached_obj, servo_obj_attached);
+static MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(servo_obj_angle_obj, 1, 2, servo_obj_angle);
+static MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(servo_obj_usecs_obj, 1, 2, servo_obj_usecs);
+
+static const mp_method_t servo_methods[] = {
+    { "attach", &servo_obj_attach_obj },
+    { "detach", &servo_obj_detach_obj },
+    { "pin", &servo_obj_pin_obj },
+    { "min_usecs", &servo_obj_min_usecs_obj },
+    { "max_usecs", &servo_obj_max_usecs_obj },
+    { "attached", &servo_obj_attached_obj },
+    { "angle", &servo_obj_angle_obj },
+    { "usecs", &servo_obj_usecs_obj },
+    { NULL, NULL },
+};
+
+/*
+ * Notes:
+ *
+ * ISR needs to know pin #, ticks
+ */
+
+static const mp_obj_type_t servo_obj_type = {
+    { &mp_const_type },
+    "Servo",
+    .print = servo_obj_print,
+    .methods = servo_methods,
+};
+
+/* servo = pyb.Servo(pin, [min_uecs, [max_usecs]]) */
+
+mp_obj_t pyb_Servo(void) {
+    uint16_t mask;
+    pyb_servo_obj_t *self = m_new_obj(pyb_servo_obj_t);
+    self->base.type = &servo_obj_type;
+    self->min_usecs = MIN_PULSE_WIDTH;
+    self->max_usecs = MAX_PULSE_WIDTH;
+
+    /* Find an unallocated servo id */
+
+    self->servo_id = 0;
+    for (mask=1; mask < (1<<MAX_SERVOS); mask <<= 1) {
+        if (!(servo_allocated_mask & mask)) {
+            servo_allocated_mask |= mask;
+            servo_active_mask &= ~mask;
+            servo_ticks[self->servo_id] = usToTicks(DEFAULT_PULSE_WIDTH);
+            return self;
+        }
+        self->servo_id++;
+    }
+    m_del_obj(pyb_servo_obj_t, self);
+    nlr_jump(mp_obj_new_exception_msg(MP_QSTR_ValueError, "No available servo ids"));
+    return mp_const_none;
+}
+
+void pdb_isr(void)
+{
+	static int8_t channel = 0, channel_high = MAX_SERVOS;
+	static uint32_t tick_accum = 0;
+	uint32_t ticks;
+	int32_t wait_ticks;
+
+	// first, if any channel was left high from the previous
+	// run, now is the time to shut it off
+	if (servo_active_mask & (1 << channel_high)) {
+		digitalWrite(servo_pin[channel_high], LOW);
+		channel_high = MAX_SERVOS;
+	}
+	// search for the next channel to turn on
+	while (channel < MAX_SERVOS) {
+		if (servo_active_mask & (1 << channel)) {
+			digitalWrite(servo_pin[channel], HIGH);
+			channel_high = channel;
+			ticks = servo_ticks[channel];
+			tick_accum += ticks;
+			PDB0_IDLY += ticks;
+			PDB0_SC = PDB_CONFIG | PDB_SC_LDOK;
+			channel++;
+			return;
+		}
+		channel++;
+	}
+	// when all channels have output, wait for the
+	// minimum refresh interval
+	wait_ticks = usToTicks(REFRESH_INTERVAL) - tick_accum;
+	if (wait_ticks < usToTicks(100)) wait_ticks = usToTicks(100);
+	else if (wait_ticks > 60000) wait_ticks = 60000;
+	tick_accum += wait_ticks;
+	PDB0_IDLY += wait_ticks;
+	PDB0_SC = PDB_CONFIG | PDB_SC_LDOK;
+	// if this wait is enough to satisfy the refresh
+	// interval, next time begin again at channel zero
+	if (tick_accum >= usToTicks(REFRESH_INTERVAL)) {
+		tick_accum = 0;
+		channel = 0;
+	}
+}
diff --git a/teensy/servo.h b/teensy/servo.h
new file mode 100644
index 0000000000..c4a5bd49b1
--- /dev/null
+++ b/teensy/servo.h
@@ -0,0 +1,2 @@
+mp_obj_t pyb_Servo(void);
+
diff --git a/teensy/usart.c b/teensy/usart.c
index 419dac9527..a700e8e379 100644
--- a/teensy/usart.c
+++ b/teensy/usart.c
@@ -1,30 +1,38 @@
 #include "misc.h"
+#include "mpconfig.h"
+#include "qstr.h"
+#include "obj.h"
 #include "../stm/usart.h"
 
-void usart_init(void) {
-}
+pyb_usart_t pyb_usart_global_debug = PYB_USART_NONE;
 
-bool usart_is_enabled(void) {
-    return false;
+void usart_init(pyb_usart_t usart_id, uint32_t baudrate)
+{
+    (void)usart_id;
+    (void)baudrate;
 }
 
-bool usart_rx_any(void) {
+bool usart_rx_any(pyb_usart_t usart_id)
+{
+    (void)usart_id;
     return false;
 }
 
-int usart_rx_char(void) {
+int usart_rx_char(pyb_usart_t usart_id)
+{
+    (void)usart_id;
     return 0;
 }
 
-void usart_tx_char(int c) {
-    (void)c;
-}
-
-void usart_tx_str(const char *str) {
+void usart_tx_str(pyb_usart_t usart_id, const char *str)
+{
+    (void)usart_id;
     (void)str;
 }
 
-void usart_tx_strn_cooked(const char *str, int len) {
-    (void)str;
-    (void)len;
+void usart_tx_strn_cooked(pyb_usart_t usart_id, const char *str, int len)
+{
+  (void)usart_id;
+  (void)str;
+  (void)len;
 }