Initial commit of source code.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

21 files changed, 4562 insertions, 0 deletions

diff --git a/klippy/cartesian.py b/klippy/cartesian.py
new file mode 100644
index 00000000..40840077
--- /dev/null
+++ b/klippy/cartesian.py
@@ -0,0 +1,252 @@
+# Code for handling cartesian (standard x, y, z planes) moves
+#
+# Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import math, logging, time
+import lookahead, stepper, homing
+
+# Common suffixes: _d is distance (in mm), _v is velocity (in
+#   mm/second), _t is time (in seconds), _r is ratio (scalar between
+#   0.0 and 1.0)
+
+StepList = (0, 1, 2, 3)
+
+class Move:
+    def __init__(self, kin, relsteps, speed):
+        self.kin = kin
+        self.relsteps = relsteps
+        self.junction_max = self.junction_start_max = self.junction_delta = 0.
+        # Calculate requested distance to travel (in mm)
+        steppers = self.kin.steppers
+        absrelsteps = [abs(relsteps[i]) for i in StepList]
+        stepper_d = [absrelsteps[i] * steppers[i].step_dist
+                     for i in StepList]
+        self.move_d = math.sqrt(sum([d*d for d in stepper_d[:3]]))
+        if not self.move_d:
+            self.move_d = stepper_d[3]
+            if not self.move_d:
+                return
+        # Limit velocity to max for each stepper
+        velocity_factor = min([steppers[i].max_step_velocity / absrelsteps[i]
+                               for i in StepList if absrelsteps[i]])
+        move_v = min(speed, velocity_factor * self.move_d)
+        self.junction_max = move_v**2
+        # Find max acceleration factor
+        accel_factor = min([steppers[i].max_step_accel / absrelsteps[i]
+                            for i in StepList if absrelsteps[i]])
+        accel = min(self.kin.max_accel, accel_factor * self.move_d)
+        self.junction_delta = 2.0 * self.move_d * accel
+    def calc_junction(self, prev_move):
+        # Find max start junction velocity using approximated
+        # centripetal velocity as described at:
+        # https://onehossshay.wordpress.com/2011/09/24/improving_grbl_cornering_algorithm/
+        if not prev_move.move_d or self.relsteps[2] or prev_move.relsteps[2]:
+            return
+        steppers = self.kin.steppers
+        junction_cos_theta = -sum([
+            self.relsteps[i] * prev_move.relsteps[i] * steppers[i].step_dist**2
+            for i in range(2)]) / (self.move_d * prev_move.move_d)
+        if junction_cos_theta > 0.999999:
+            return
+        junction_cos_theta = max(junction_cos_theta, -0.999999)
+        sin_theta_d2 = math.sqrt(0.5*(1.0-junction_cos_theta));
+        R = self.kin.junction_deviation * sin_theta_d2 / (1.0 - sin_theta_d2)
+        accel = self.junction_delta / (2.0 * self.move_d)
+        self.junction_start_max = min(
+            accel * R, self.junction_max, prev_move.junction_max)
+    def process(self, junction_start, junction_end):
+        # Determine accel, cruise, and decel portions of the move
+        junction_cruise = self.junction_max
+        inv_junction_delta = 1. / self.junction_delta
+        accel_r = (junction_cruise-junction_start) * inv_junction_delta
+        decel_r = (junction_cruise-junction_end) * inv_junction_delta
+        cruise_r = 1. - accel_r - decel_r
+        if cruise_r < 0.:
+            accel_r += 0.5 * cruise_r
+            decel_r = 1.0 - accel_r
+            cruise_r = 0.
+            junction_cruise = junction_start + accel_r*self.junction_delta
+        # Determine the move velocities and time spent in each portion
+        start_v = math.sqrt(junction_start)
+        cruise_v = math.sqrt(junction_cruise)
+        end_v = math.sqrt(junction_end)
+        inv_cruise_v = 1. / cruise_v
+        inv_accel = 2.0 * self.move_d * inv_junction_delta
+        accel_t = 2.0 * self.move_d * accel_r / (start_v+cruise_v)
+        cruise_t = self.move_d * cruise_r * inv_cruise_v
+        decel_t = 2.0 * self.move_d * decel_r / (end_v+cruise_v)
+
+        #logging.debug("Move: %s v=%.2f/%.2f/%.2f mt=%.3f st=%.3f %.3f %.3f" % (
+        #    self.relsteps, start_v, cruise_v, end_v, move_t
+        #    , next_move_time, accel_r, cruise_r))
+
+        # Calculate step times for the move
+        next_move_time = self.kin.get_next_move_time()
+        for i in StepList:
+            steps = self.relsteps[i]
+            if not steps:
+                continue
+            sdir = 0
+            if steps < 0:
+                sdir = 1
+                steps = -steps
+            clock_offset, clock_freq, so = self.kin.steppers[i].prep_move(
+                sdir, next_move_time)
+
+            step_dist = self.move_d / steps
+            step_offset = 0.5
+
+            # Acceleration steps
+            #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
+            accel_clock_offset = start_v * inv_accel * clock_freq
+            accel_sqrt_offset = accel_clock_offset**2
+            accel_multiplier = 2.0 * step_dist * inv_accel * clock_freq**2
+            accel_steps = accel_r * steps
+            step_offset = so.step_sqrt(
+                accel_steps, step_offset, clock_offset - accel_clock_offset
+                , accel_sqrt_offset, accel_multiplier)
+            clock_offset += accel_t * clock_freq
+            # Cruising steps
+            #t = pos/cruise_v
+            cruise_multiplier = step_dist * inv_cruise_v * clock_freq
+            cruise_steps = cruise_r * steps
+            step_offset = so.step_factor(
+                cruise_steps, step_offset, clock_offset, cruise_multiplier)
+            clock_offset += cruise_t * clock_freq
+            # Deceleration steps
+            #t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel)
+            decel_clock_offset = cruise_v * inv_accel * clock_freq
+            decel_sqrt_offset = decel_clock_offset**2
+            decel_steps = decel_r * steps
+            so.step_sqrt(
+                decel_steps, step_offset, clock_offset + decel_clock_offset
+                , decel_sqrt_offset, -accel_multiplier)
+        self.kin.update_move_time(accel_t + cruise_t + decel_t)
+
+STALL_TIME = 0.100
+
+class CartKinematics:
+    def __init__(self, printer, config):
+        self.printer = printer
+        self.reactor = printer.reactor
+        steppers = ['stepper_x', 'stepper_y', 'stepper_z', 'stepper_e']
+        self.steppers = [stepper.PrinterStepper(printer, config.getsection(n))
+                         for n in steppers]
+        self.max_accel = min(s.max_step_accel*s.step_dist
+                             for s in self.steppers[:2]) # XXX
+        dummy_move = Move(self, [0]*len(self.steppers), 0.)
+        dummy_move.junction_max = 0.
+        self.junction_deviation = config.getfloat('junction_deviation', 0.02)
+        self.move_queue = lookahead.MoveQueue(dummy_move)
+        self.pos = [0, 0, 0, 0]
+        # Print time tracking
+        self.buffer_time_high = config.getfloat('buffer_time_high', 5.000)
+        self.buffer_time_low = config.getfloat('buffer_time_low', 0.150)
+        self.move_flush_time = config.getfloat('move_flush_time', 0.050)
+        self.motor_off_delay = config.getfloat('motor_off_time', 60.000)
+        self.print_time = 0.
+        self.print_time_stall = 0
+        self.motor_off_time = self.reactor.NEVER
+        self.flush_timer = self.reactor.register_timer(self.flush_handler)
+    def build_config(self):
+        for stepper in self.steppers:
+            stepper.build_config()
+    # Print time tracking
+    def update_move_time(self, movetime):
+        self.print_time += movetime
+        flush_to_time = self.print_time - self.move_flush_time
+        self.printer.mcu.flush_moves(flush_to_time)
+    def get_next_move_time(self):
+        if not self.print_time:
+            self.print_time = self.buffer_time_low + STALL_TIME
+            curtime = time.time()
+            self.printer.mcu.set_print_start_time(curtime)
+            self.reactor.update_timer(self.flush_timer, self.reactor.NOW)
+        return self.print_time
+    def get_last_move_time(self):
+        self.move_queue.flush()
+        return self.get_next_move_time()
+    def reset_motor_off_time(self, eventtime):
+        self.motor_off_time = eventtime + self.motor_off_delay
+    def reset_print_time(self):
+        self.move_queue.flush()
+        self.printer.mcu.flush_moves(self.print_time)
+        self.print_time = 0.
+        self.reset_motor_off_time(time.time())
+        self.reactor.update_timer(self.flush_timer, self.motor_off_time)
+    def check_busy(self, eventtime):
+        if not self.print_time:
+            # XXX - find better way to flush initial move_queue items
+            if self.move_queue.queue:
+                self.reactor.update_timer(self.flush_timer, eventtime + 0.100)
+            return False
+        buffer_time = self.printer.mcu.get_print_buffer_time(
+            eventtime, self.print_time)
+        return buffer_time > self.buffer_time_high
+    def flush_handler(self, eventtime):
+        if not self.print_time:
+            self.move_queue.flush()
+            if not self.print_time:
+                if eventtime >= self.motor_off_time:
+                    self.motor_off()
+                    self.reset_print_time()
+                    self.motor_off_time = self.reactor.NEVER
+                return self.motor_off_time
+        print_time = self.print_time
+        buffer_time = self.printer.mcu.get_print_buffer_time(
+            eventtime, print_time)
+        if buffer_time > self.buffer_time_low:
+            return eventtime + buffer_time - self.buffer_time_low
+        self.move_queue.flush()
+        if print_time != self.print_time:
+            self.print_time_stall += 1
+            self.dwell(self.buffer_time_low + STALL_TIME)
+            return self.reactor.NOW
+        self.reset_print_time()
+        return self.motor_off_time
+    def stats(self, eventtime):
+        buffer_time = 0.
+        if self.print_time:
+            buffer_time = self.printer.mcu.get_print_buffer_time(
+                eventtime, self.print_time)
+        return "print_time=%.3f buffer_time=%.3f print_time_stall=%d" % (
+            self.print_time, buffer_time, self.print_time_stall)
+    # Movement commands
+    def get_position(self):
+        return [self.pos[i] * self.steppers[i].step_dist
+                for i in StepList]
+    def set_position(self, newpos):
+        self.pos = [int(newpos[i]*self.steppers[i].inv_step_dist + 0.5)
+                    for i in StepList]
+    def move(self, newpos, speed, sloppy=False):
+        # Round to closest step position
+        newpos = [int(newpos[i]*self.steppers[i].inv_step_dist + 0.5)
+                  for i in StepList]
+        relsteps = [newpos[i] - self.pos[i] for i in StepList]
+        self.pos = newpos
+        if relsteps == [0]*len(newpos):
+            # no move
+            return
+        #logging.debug("; dist %s @ %d\n" % (
+        #    [newpos[i]*self.steppers[i].step_dist for i in StepList], speed))
+        # Create move and queue it
+        move = Move(self, relsteps, speed)
+        move.calc_junction(self.move_queue.prev_move())
+        self.move_queue.add_move(move)
+    def home(self, axis):
+        # Each axis is homed independently and in order
+        homing_state = homing.Homing(self, self.steppers)
+        for a in axis:
+            homing_state.plan_home(a)
+        return homing_state
+    def dwell(self, delay):
+        self.get_last_move_time()
+        self.update_move_time(delay)
+    def motor_off(self):
+        self.dwell(STALL_TIME)
+        last_move_time = self.get_last_move_time()
+        for stepper in self.steppers:
+            stepper.motor_enable(last_move_time, 0)
+        self.dwell(STALL_TIME)
+        logging.debug('; Max time of %f' % (last_move_time,))
diff --git a/klippy/chelper.py b/klippy/chelper.py
new file mode 100644
index 00000000..99ec3f4f
--- /dev/null
+++ b/klippy/chelper.py
@@ -0,0 +1,95 @@
+# Wrapper around C helper code
+#
+# Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import os, logging
+import cffi
+
+COMPILE_CMD = "gcc -Wall -g -O -shared -fPIC -o %s %s"
+SOURCE_FILES = ['stepcompress.c', 'serialqueue.c']
+DEST_LIB = "c_helper.so"
+OTHER_FILES = ['list.h', 'serialqueue.h']
+
+defs_stepcompress = """
+    struct stepcompress *stepcompress_alloc(uint32_t max_error
+        , uint32_t queue_step_msgid, uint32_t oid);
+    void stepcompress_push(struct stepcompress *sc, double step_clock);
+    double stepcompress_push_factor(struct stepcompress *sc
+        , double steps, double step_offset
+        , double clock_offset, double factor);
+    double stepcompress_push_sqrt(struct stepcompress *sc
+        , double steps, double step_offset
+        , double clock_offset, double sqrt_offset, double factor);
+    void stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock);
+    void stepcompress_queue_msg(struct stepcompress *sc
+        , uint32_t *data, int len);
+    uint32_t stepcompress_get_errors(struct stepcompress *sc);
+
+    struct steppersync *steppersync_alloc(struct serialqueue *sq
+        , struct stepcompress **sc_list, int sc_num, int move_num);
+    void steppersync_flush(struct steppersync *ss, uint64_t move_clock);
+"""
+
+defs_serialqueue = """
+    #define MESSAGE_MAX 64
+    struct pull_queue_message {
+        uint8_t msg[MESSAGE_MAX];
+        int len;
+        double sent_time, receive_time;
+    };
+
+    struct serialqueue *serialqueue_alloc(int serial_fd, double baud_adjust
+        , int write_only);
+    void serialqueue_exit(struct serialqueue *sq);
+    struct command_queue *serialqueue_alloc_commandqueue(void);
+    void serialqueue_send(struct serialqueue *sq, struct command_queue *cq
+        , uint8_t *msg, int len, uint64_t min_clock, uint64_t req_clock);
+    void serialqueue_encode_and_send(struct serialqueue *sq
+        , struct command_queue *cq, uint32_t *data, int len
+        , uint64_t min_clock, uint64_t req_clock);
+    void serialqueue_pull(struct serialqueue *sq, struct pull_queue_message *pqm);
+    void serialqueue_set_clock_est(struct serialqueue *sq, double est_clock
+        , double last_ack_time, uint64_t last_ack_clock);
+    void serialqueue_flush_ready(struct serialqueue *sq);
+    void serialqueue_get_stats(struct serialqueue *sq, char *buf, int len);
+    int serialqueue_extract_old(struct serialqueue *sq, int sentq
+        , struct pull_queue_message *q, int max);
+"""
+
+# Return the list of file modification times
+def get_mtimes(srcdir, filelist):
+    out = []
+    for filename in filelist:
+        pathname = os.path.join(srcdir, filename)
+        try:
+            t = os.path.getmtime(pathname)
+        except os.error:
+            continue
+        out.append(t)
+    return out
+
+# Check if the code needs to be compiled
+def check_build_code(srcdir):
+    src_times = get_mtimes(srcdir, SOURCE_FILES + OTHER_FILES)
+    obj_times = get_mtimes(srcdir, [DEST_LIB])
+    if not obj_times or max(src_times) > min(obj_times):
+        logging.info("Building C code module")
+        srcfiles = [os.path.join(srcdir, fname) for fname in SOURCE_FILES]
+        destlib = os.path.join(srcdir, DEST_LIB)
+        os.system(COMPILE_CMD % (destlib, ' '.join(srcfiles)))
+
+FFI_main = None
+FFI_lib = None
+
+# Return the Foreign Function Interface api to the caller
+def get_ffi():
+    global FFI_main, FFI_lib
+    if FFI_lib is None:
+        srcdir = os.path.dirname(os.path.realpath(__file__))
+        check_build_code(srcdir)
+        FFI_main = cffi.FFI()
+        FFI_main.cdef(defs_stepcompress)
+        FFI_main.cdef(defs_serialqueue)
+        FFI_lib = FFI_main.dlopen(os.path.join(srcdir, DEST_LIB))
+    return FFI_main, FFI_lib
diff --git a/klippy/console.py b/klippy/console.py
new file mode 100755
index 00000000..4782702a
--- /dev/null
+++ b/klippy/console.py
@@ -0,0 +1,102 @@
+#!/usr/bin/env python
+# Script to implement a test console with firmware over serial port
+#
+# Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import sys, optparse, os, re, logging
+
+import reactor, serialhdl, pins, util, msgproto
+
+re_eval = re.compile(r'\{(?P<eval>[^}]*)\}')
+
+class KeyboardReader:
+    def __init__(self, ser, reactor):
+        self.ser = ser
+        self.reactor = reactor
+        self.fd = sys.stdin.fileno()
+        util.set_nonblock(self.fd)
+        self.pins = None
+        self.data = ""
+        self.reactor.register_fd(self.fd, self.process_kbd)
+        self.local_commands = { "PINS": self.set_pin_map }
+        self.eval_globals = {}
+    def update_evals(self, eventtime):
+        f = self.ser.msgparser.config.get('CLOCK_FREQ', 1)
+        c = (eventtime - self.ser.last_ack_time) * f + self.ser.last_ack_clock
+        self.eval_globals['freq'] = f
+        self.eval_globals['clock'] = int(c)
+    def set_pin_map(self, parts):
+        mcu = self.ser.msgparser.config['MCU']
+        self.pins = pins.map_pins(parts[1], mcu)
+    def lookup_pin(self, value):
+        if self.pins is None:
+            self.pins = pins.mcu_to_pins(self.ser.msgparser.config['MCU'])
+        return self.pins[value]
+    def translate(self, line, eventtime):
+        evalparts = re_eval.split(line)
+        if len(evalparts) > 1:
+            self.update_evals(eventtime)
+            try:
+                for i in range(1, len(evalparts), 2):
+                    evalparts[i] = str(eval(evalparts[i], self.eval_globals))
+            except:
+                print "Unable to evaluate: ", line
+                return None
+            line = ''.join(evalparts)
+            print "Eval:", line
+        if self.pins is None and self.ser.msgparser.config:
+            self.pins = pins.mcu_to_pins(self.ser.msgparser.config['MCU'])
+        if self.pins is not None:
+            try:
+                line = pins.update_command(line, self.pins).strip()
+            except:
+                print "Unable to map pin: ", line
+                return None
+        if line:
+            parts = line.split()
+            if parts[0] in self.local_commands:
+                self.local_commands[parts[0]](parts)
+                return None
+        try:
+            msg = self.ser.msgparser.create_command(line)
+        except msgproto.error, e:
+            print "Error:", e
+            return None
+        return msg
+    def process_kbd(self, eventtime):
+        self.data += os.read(self.fd, 4096)
+
+        kbdlines = self.data.split('\n')
+        for line in kbdlines[:-1]:
+            line = line.strip()
+            cpos = line.find('#')
+            if cpos >= 0:
+                line = line[:cpos]
+                if not line:
+                    continue
+            msg = self.translate(line.strip(), eventtime)
+            if msg is None:
+                continue
+            self.ser.send(msg)
+        self.data = kbdlines[-1]
+
+def main():
+    usage = "%prog [options] <serialdevice> <baud>"
+    opts = optparse.OptionParser(usage)
+    options, args = opts.parse_args()
+    serialport, baud = args
+    baud = int(baud)
+
+    logging.basicConfig(level=logging.DEBUG)
+    r = reactor.Reactor()
+    ser = serialhdl.SerialReader(r, serialport, baud)
+    ser.connect()
+    kbd = KeyboardReader(ser, r)
+    try:
+        r.run()
+    except KeyboardInterrupt:
+        sys.stdout.write("\n")
+
+if __name__ == '__main__':
+    main()
diff --git a/klippy/fan.py b/klippy/fan.py
new file mode 100644
index 00000000..52374844
--- /dev/null
+++ b/klippy/fan.py
@@ -0,0 +1,39 @@
+# Printer fan support
+#
+# Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+
+FAN_MIN_TIME = 0.1
+
+class PrinterFan:
+    def __init__(self, printer, config):
+        self.printer = printer
+        self.config = config
+        self.mcu_fan = None
+        self.last_fan_clock = self.last_fan_value = 0
+        self.min_fan_clock = 0
+        self.kick_start_clock = 0
+    def build_config(self):
+        pin = self.config.get('pin')
+        hard_pwm = self.config.getint('hard_pwm', 128)
+        mcu_freq = self.printer.mcu.get_mcu_freq()
+        self.min_fan_clock = int(FAN_MIN_TIME * mcu_freq)
+        kst = self.config.getfloat('kick_start_time', 0.1)
+        self.kick_start_clock = int(kst * mcu_freq)
+        self.mcu_fan = self.printer.mcu.create_pwm(pin, hard_pwm, 0)
+    # External commands
+    def set_speed(self, print_time, value):
+        value = max(0, min(255, int(value*255. + 0.5)))
+        if value == self.last_fan_value:
+            return
+        pc = int(self.mcu_fan.get_print_clock(print_time))
+        pc = max(self.last_fan_clock + self.min_fan_clock, pc)
+        if (value and value < 255
+            and not self.last_fan_value and self.kick_start_clock):
+            # Run fan at full speed for specified kick_start_time
+            self.mcu_fan.set_pwm(pc, 255)
+            pc += self.kick_start_clock
+        self.mcu_fan.set_pwm(pc, value)
+        self.last_fan_clock = pc
+        self.last_fan_value = value
diff --git a/klippy/gcode.py b/klippy/gcode.py
new file mode 100644
index 00000000..fc697b98
--- /dev/null
+++ b/klippy/gcode.py
@@ -0,0 +1,315 @@
+# Parse gcode commands
+#
+# Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import os, re, logging
+
+# Parse out incoming GCode and find and translate head movements
+class GCodeParser:
+    RETRY_TIME = 0.100
+    def __init__(self, printer, fd, inputfile=False):
+        self.printer = printer
+        self.fd = fd
+        self.inputfile = inputfile
+        # Input handling
+        self.reactor = printer.reactor
+        self.fd_handle = None
+        self.input_commands = [""]
+        self.need_register_fd = False
+        self.bytes_read = 0
+        # Busy handling
+        self.busy_timer = self.reactor.register_timer(self.busy_handler)
+        self.busy_state = None
+        # Command handling
+        self.gcode_handlers = {}
+        self.is_shutdown = False
+        self.need_ack = False
+        self.kin = self.heater_nozzle = self.heater_bed = self.fan = None
+        self.movemult = 1.0
+        self.speed = 1.0
+        self.absolutecoord = self.absoluteextrude = True
+        self.base_position = [0.0, 0.0, 0.0, 0.0]
+        self.last_position = [0.0, 0.0, 0.0, 0.0]
+        self.homing_add = [0.0, 0.0, 0.0, 0.0]
+        self.axis2pos = {'X': 0, 'Y': 1, 'Z': 2, 'E': 3}
+    def build_config(self):
+        self.kin = self.printer.objects['kinematics']
+        self.heater_nozzle = self.printer.objects.get('heater_nozzle')
+        self.heater_bed = self.printer.objects.get('heater_bed')
+        self.fan = self.printer.objects.get('fan')
+        self.build_handlers()
+    def build_handlers(self):
+        shutdown_handlers = ['M105', 'M110', 'M114']
+        handlers = ['G0', 'G1', 'G4', 'G20', 'G21', 'G28', 'G90', 'G91', 'G92',
+                    'M18', 'M82', 'M83', 'M84', 'M110', 'M114', 'M206']
+        if self.heater_nozzle is not None:
+            handlers.extend(['M104', 'M105', 'M109', 'M303'])
+        if self.heater_bed is not None:
+            handlers.extend(['M140', 'M190'])
+        if self.fan is not None:
+            handlers.extend(['M106', 'M107'])
+        if self.is_shutdown:
+            handlers = [h for h in handlers if h in shutdown_handlers]
+        self.gcode_handlers = dict((h, getattr(self, 'cmd_'+h))
+                                   for h in handlers)
+    def run(self):
+        if self.heater_nozzle is not None:
+            self.heater_nozzle.run()
+        if self.heater_bed is not None:
+            self.heater_bed.run()
+        self.fd_handle = self.reactor.register_fd(self.fd, self.process_data)
+        self.reactor.run()
+    def finish(self):
+        self.reactor.end()
+        self.kin.motor_off()
+        logging.debug('Completed translation by klippy')
+    def stats(self, eventtime):
+        return "gcodein=%d" % (self.bytes_read,)
+    def shutdown(self):
+        self.is_shutdown = True
+        self.build_handlers()
+    # Parse input into commands
+    args_r = re.compile('([a-zA-Z*])')
+    def process_commands(self, eventtime):
+        i = -1
+        for i in range(len(self.input_commands)-1):
+            line = self.input_commands[i]
+            # Ignore comments and leading/trailing spaces
+            line = origline = line.strip()
+            cpos = line.find(';')
+            if cpos >= 0:
+                line = line[:cpos]
+            # Break command into parts
+            parts = self.args_r.split(line)[1:]
+            params = dict((parts[i].upper(), parts[i+1].strip())
+                          for i in range(0, len(parts), 2))
+            params['#original'] = origline
+            if parts and parts[0].upper() == 'N':
+                # Skip line number at start of command
+                del parts[:2]
+            if not parts:
+                self.cmd_default(params)
+                continue
+            params['#command'] = cmd = parts[0] + parts[1].strip()
+            # Invoke handler for command
+            self.need_ack = True
+            handler = self.gcode_handlers.get(cmd, self.cmd_default)
+            try:
+                handler(params)
+            except:
+                logging.exception("Exception in command handler")
+                self.respond('echo:Internal error on command:"%s"' % (cmd,))
+            # Check if machine can process next command or must stall input
+            if self.busy_state is not None:
+                break
+            if self.kin.check_busy(eventtime):
+                self.set_busy(self.kin)
+                break
+            self.ack()
+        del self.input_commands[:i+1]
+    def process_data(self, eventtime):
+        if self.busy_state is not None:
+            self.reactor.unregister_fd(self.fd_handle)
+            self.need_register_fd = True
+            return
+        data = os.read(self.fd, 4096)
+        self.bytes_read += len(data)
+        lines = data.split('\n')
+        lines[0] = self.input_commands[0] + lines[0]
+        self.input_commands = lines
+        self.process_commands(eventtime)
+        if not data and self.inputfile:
+            self.finish()
+    # Response handling
+    def ack(self, msg=None):
+        if not self.need_ack or self.inputfile:
+            return
+        if msg:
+            os.write(self.fd, "ok %s\n" % (msg,))
+        else:
+            os.write(self.fd, "ok\n")
+        self.need_ack = False
+    def respond(self, msg):
+        logging.debug(msg)
+        if self.inputfile:
+            return
+        os.write(self.fd, msg+"\n")
+    # Busy handling
+    def set_busy(self, busy_handler):
+        self.busy_state = busy_handler
+        self.reactor.update_timer(self.busy_timer, self.reactor.NOW)
+    def busy_handler(self, eventtime):
+        busy = self.busy_state.check_busy(eventtime)
+        if busy:
+            self.kin.reset_motor_off_time(eventtime)
+            return eventtime + self.RETRY_TIME
+        self.busy_state = None
+        self.ack()
+        self.process_commands(eventtime)
+        if self.busy_state is not None:
+            return self.reactor.NOW
+        if self.need_register_fd:
+            self.need_register_fd = False
+            self.fd_handle = self.reactor.register_fd(self.fd, self.process_data)
+        return self.reactor.NEVER
+    # Temperature wrappers
+    def get_temp(self):
+        # T:XXX /YYY B:XXX /YYY
+        out = []
+        if self.heater_nozzle:
+            cur, target = self.heater_nozzle.get_temp()
+            out.append("T:%.1f /%.1f" % (cur, target))
+        if self.heater_bed:
+            cur, target = self.heater_bed.get_temp()
+            out.append("B:%.1f /%.1f" % (cur, target))
+        return " ".join(out)
+    def bg_temp(self, heater):
+        # Wrapper class for check_busy() that periodically prints current temp
+        class temp_busy_handler_wrapper:
+            gcode = self
+            last_temp_time = 0.
+            cur_heater = heater
+            def check_busy(self, eventtime):
+                if eventtime > self.last_temp_time + 1.0:
+                    self.gcode.respond(self.gcode.get_temp())
+                    self.last_temp_time = eventtime
+                return self.cur_heater.check_busy(eventtime)
+        if self.inputfile:
+            return
+        self.set_busy(temp_busy_handler_wrapper())
+    def set_temp(self, heater, params, wait=False):
+        print_time = self.kin.get_last_move_time()
+        temp = float(params.get('S', '0'))
+        heater.set_temp(print_time, temp)
+        if wait:
+            self.bg_temp(heater)
+    # Individual command handlers
+    def cmd_default(self, params):
+        if self.is_shutdown:
+            self.respond('Error: Machine is shutdown')
+            return
+        cmd = params.get('#command')
+        if not cmd:
+            logging.debug(params['#original'])
+            return
+        self.respond('echo:Unknown command:"%s"' % (cmd,))
+    def cmd_G0(self, params):
+        self.cmd_G1(params, sloppy=True)
+    def cmd_G1(self, params, sloppy=False):
+        # Move
+        for a, p in self.axis2pos.items():
+            if a in params:
+                v = float(params[a])
+                if not self.absolutecoord or (p>2 and not self.absoluteextrude):
+                    # value relative to position of last move
+                    self.last_position[p] += v
+                else:
+                    # value relative to base coordinate position
+                    self.last_position[p] = v + self.base_position[p]
+        if 'F' in params:
+            self.speed = float(params['F']) / 60.
+        self.kin.move(self.last_position, self.speed, sloppy)
+    def cmd_G4(self, params):
+        # Dwell
+        if 'S' in params:
+            delay = float(params['S'])
+        else:
+            delay = float(params.get('P', '0')) / 1000.
+        self.kin.dwell(delay)
+    def cmd_G20(self, params):
+        # Set units to inches
+        self.movemult = 25.4
+    def cmd_G21(self, params):
+        # Set units to millimeters
+        self.movemult = 1.0
+    def cmd_G28(self, params):
+        # Move to origin
+        axis = []
+        for a in 'XYZ':
+            if a in params:
+                axis.append(self.axis2pos[a])
+        if not axis:
+            axis = [0, 1, 2]
+        busy_handler = self.kin.home(axis)
+        def axis_update(axis):
+            newpos = self.kin.get_position()
+            for a in axis:
+                self.last_position[a] = newpos[a]
+                self.base_position[a] = -self.homing_add[a]
+        busy_handler.plan_axis_update(axis_update)
+        self.set_busy(busy_handler)
+    def cmd_G90(self, params):
+        # Use absolute coordinates
+        self.absolutecoord = True
+    def cmd_G91(self, params):
+        # Use relative coordinates
+        self.absolutecoord = False
+    def cmd_G92(self, params):
+        # Set position
+        mcount = 0
+        for a, p in self.axis2pos.items():
+            if a in params:
+                self.base_position[p] = self.last_position[p] - float(params[a])
+                mcount += 1
+        if not mcount:
+            self.base_position = list(self.last_position)
+    def cmd_M82(self, params):
+        # Use absolute distances for extrusion
+        self.absoluteextrude = True
+    def cmd_M83(self, params):
+        # Use relative distances for extrusion
+        self.absoluteextrude = False
+    def cmd_M18(self, params):
+        # Turn off motors
+        self.kin.motor_off()
+    def cmd_M84(self, params):
+        # Stop idle hold
+        self.kin.motor_off()
+    def cmd_M105(self, params):
+        # Get Extruder Temperature
+        self.ack(self.get_temp())
+    def cmd_M104(self, params):
+        # Set Extruder Temperature
+        self.set_temp(self.heater_nozzle, params)
+    def cmd_M109(self, params):
+        # Set Extruder Temperature and Wait
+        self.set_temp(self.heater_nozzle, params, wait=True)
+    def cmd_M110(self, params):
+        # Set Current Line Number
+        pass
+    def cmd_M114(self, params):
+        # Get Current Position
+        kinpos = self.kin.get_position()
+        self.respond("X:%.3f Y:%.3f Z:%.3f E:%.3f Count X:%.3f Y:%.3f Z:%.3f" % (
+            self.last_position[0], self.last_position[1],
+            self.last_position[2], self.last_position[3],
+            kinpos[0], kinpos[1], kinpos[2]))
+    def cmd_M140(self, params):
+        # Set Bed Temperature
+        self.set_temp(self.heater_bed, params)
+    def cmd_M190(self, params):
+        # Set Bed Temperature and Wait
+        self.set_temp(self.heater_bed, params, wait=True)
+    def cmd_M106(self, params):
+        # Set fan speed
+        print_time = self.kin.get_last_move_time()
+        self.fan.set_speed(print_time, float(params.get('S', '255')) / 255.)
+    def cmd_M107(self, params):
+        # Turn fan off
+        print_time = self.kin.get_last_move_time()
+        self.fan.set_speed(print_time, 0)
+    def cmd_M206(self, params):
+        # Set home offset
+        for a, p in self.axis2pos.items():
+            if a in params:
+                v = float(params[a])
+                self.base_position[p] += self.homing_add[p] - v
+                self.homing_add[p] = v
+    def cmd_M303(self, params):
+        # Run PID tuning
+        heater = int(params.get('E', '0'))
+        heater = {0: self.heater_nozzle, -1: self.heater_bed}[heater]
+        temp = float(params.get('S', '60'))
+        heater.start_auto_tune(temp)
+        self.bg_temp(heater)
diff --git a/klippy/heater.py b/klippy/heater.py
new file mode 100644
index 00000000..d05035a0
--- /dev/null
+++ b/klippy/heater.py
@@ -0,0 +1,288 @@
+# Printer heater support
+#
+# Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import math, logging, threading
+
+# Mapping from name to Steinhart-Hart coefficients
+Thermistors = {
+    "EPCOS 100K B57560G104F": (
+        0.000722136308968056, 0.000216766566488498, 8.92935804531095e-08)
+}
+
+SAMPLE_TIME = 0.001
+SAMPLE_COUNT = 8
+REPORT_TIME = 0.300
+KELVIN_TO_CELCIUS = -273.15
+MAX_HEAT_TIME = 5.0
+AMBIENT_TEMP = 25.
+PWM_MAX = 255
+
+class PrinterHeater:
+    def __init__(self, printer, config):
+        self.printer = printer
+        self.config = config
+        self.mcu_pwm = self.mcu_adc = None
+        self.thermistor_c = Thermistors.get(config.get('thermistor_type'))
+        self.pullup_r = config.getfloat('pullup_resistor', 4700.)
+        self.lock = threading.Lock()
+        self.last_temp = 0.
+        self.last_temp_clock = 0
+        self.target_temp = 0.
+        self.report_clock = 0
+        self.control = None
+        # pwm caching
+        self.next_pwm_clock = 0
+        self.last_pwm_value = 0
+        self.resend_clock = 0
+        self.pwm_offset_clock = 0
+    def build_config(self):
+        heater_pin = self.config.get('heater_pin')
+        thermistor_pin = self.config.get('thermistor_pin')
+        self.mcu_pwm = self.printer.mcu.create_pwm(heater_pin, 0, MAX_HEAT_TIME)
+        self.mcu_adc = self.printer.mcu.create_adc(thermistor_pin)
+        min_adc = self.calc_adc(self.config.getfloat('max_temp'))
+        max_adc = self.calc_adc(self.config.getfloat('min_temp'))
+        freq = self.printer.mcu.get_mcu_freq()
+        sample_clock = int(SAMPLE_TIME*freq)
+        self.mcu_adc.set_minmax(
+            sample_clock, SAMPLE_COUNT, minval=min_adc, maxval=max_adc)
+        self.mcu_adc.set_adc_callback(self.adc_callback)
+        self.report_clock = int(REPORT_TIME*freq)
+        control_algo = self.config.get('control', 'watermark')
+        algos = {'watermark': ControlBangBang, 'pid': ControlPID}
+        self.control = algos[control_algo](self, self.config)
+        self.next_pwm_clock = 0
+        self.last_pwm_value = 0
+        self.resend_clock = int(MAX_HEAT_TIME * freq * 3. / 4.)
+        self.pwm_offset_clock = sample_clock*SAMPLE_COUNT + self.report_clock
+    def run(self):
+        self.mcu_adc.query_analog_in(self.report_clock)
+    def set_pwm(self, read_clock, value):
+        if value:
+            if self.target_temp <= 0.:
+                return
+            if (read_clock < self.next_pwm_clock
+                and abs(value - self.last_pwm_value) < 15):
+                return
+        elif not self.last_pwm_value:
+            return
+        pwm_clock = read_clock + self.pwm_offset_clock
+        self.next_pwm_clock = pwm_clock + self.resend_clock
+        self.last_pwm_value = value
+        logging.debug("pwm=%d@%d (%d)" % (value, read_clock, pwm_clock))
+        self.mcu_pwm.set_pwm(pwm_clock, value)
+    # Temperature calculation
+    def calc_temp(self, adc):
+        r = self.pullup_r * adc / (1.0 - adc)
+        ln_r = math.log(r)
+        c1, c2, c3 = self.thermistor_c
+        temp_inv = c1 + c2*ln_r + c3*math.pow(ln_r, 3)
+        return 1.0/temp_inv + KELVIN_TO_CELCIUS
+    def calc_adc(self, temp):
+        if temp is None:
+            return None
+        c1, c2, c3 = self.thermistor_c
+        temp -= KELVIN_TO_CELCIUS
+        temp_inv = 1./temp
+        y = (c1 - temp_inv) / (2*c3)
+        x = math.sqrt(math.pow(c2 / (3.*c3), 3.) + math.pow(y, 2.))
+        r = math.exp(math.pow(x-y, 1./3.) - math.pow(x+y, 1./3.))
+        return r / (self.pullup_r + r)
+    def adc_callback(self, read_clock, read_value):
+        temp = self.calc_temp(float(read_value))
+        with self.lock:
+            self.last_temp = temp
+            self.last_temp_clock = read_clock
+            self.control.adc_callback(read_clock, temp)
+        #logging.debug("temp: %d(%d) %f = %f" % (
+        #    read_clock, read_clock & 0xffffffff, read_value, temp))
+    # External commands
+    def set_temp(self, print_time, degrees):
+        with self.lock:
+            self.target_temp = degrees
+    def get_temp(self):
+        with self.lock:
+            return self.last_temp, self.target_temp
+    def check_busy(self, eventtime):
+        with self.lock:
+            return self.control.check_busy(eventtime)
+    def start_auto_tune(self, temp):
+        with self.lock:
+            self.control = ControlAutoTune(self, self.control, temp)
+
+
+######################################################################
+# Bang-bang control algo
+######################################################################
+
+class ControlBangBang:
+    def __init__(self, heater, config):
+        self.heater = heater
+        self.max_delta = config.getfloat('max_delta', 2.0)
+        self.heating = False
+    def adc_callback(self, read_clock, temp):
+        if self.heating and temp >= self.heater.target_temp+self.max_delta:
+            self.heating = False
+        elif not self.heating and temp <= self.heater.target_temp-self.max_delta:
+            self.heating = True
+        if self.heating:
+            self.heater.set_pwm(read_clock, PWM_MAX)
+        else:
+            self.heater.set_pwm(read_clock, 0)
+    def check_busy(self, eventtime):
+        return self.heater.last_temp < self.heater.target_temp-self.max_delta
+
+
+######################################################################
+# Proportional Integral Derivative (PID) control algo
+######################################################################
+
+class ControlPID:
+    def __init__(self, heater, config):
+        self.heater = heater
+        self.Kp = config.getfloat('pid_Kp')
+        self.Ki = config.getfloat('pid_Ki')
+        self.Kd = config.getfloat('pid_Kd')
+        self.min_deriv_time = config.getfloat('pid_deriv_time', 2.)
+        imax = config.getint('pid_integral_max', PWM_MAX)
+        self.temp_integ_max = imax / self.Ki
+        self.prev_temp = AMBIENT_TEMP
+        self.prev_temp_clock = 0
+        self.prev_temp_deriv = 0.
+        self.prev_temp_integ = 0.
+        self.inv_mcu_freq = 1. / self.heater.printer.mcu.get_mcu_freq()
+    def adc_callback(self, read_clock, temp):
+        time_diff = (read_clock - self.prev_temp_clock) * self.inv_mcu_freq
+        # Calculate change of temperature
+        temp_diff = temp - self.prev_temp
+        if time_diff >= self.min_deriv_time:
+            temp_deriv = temp_diff / time_diff
+        else:
+            temp_deriv = (self.prev_temp_deriv * (self.min_deriv_time-time_diff)
+                          + temp_diff) / self.min_deriv_time
+        # Calculate accumulated temperature "error"
+        temp_err = self.heater.target_temp - temp
+        temp_integ = self.prev_temp_integ + temp_err * time_diff
+        temp_integ = max(0., min(self.temp_integ_max, temp_integ))
+        # Calculate output
+        co = int(self.Kp*temp_err + self.Ki*temp_integ - self.Kd*temp_deriv)
+        #logging.debug("pid: %f@%d -> diff=%f deriv=%f err=%f integ=%f co=%d" % (
+        #    temp, read_clock, temp_diff, temp_deriv, temp_err, temp_integ, co))
+        bounded_co = max(0, min(PWM_MAX, co))
+        self.heater.set_pwm(read_clock, bounded_co)
+        # Store state for next measurement
+        self.prev_temp = temp
+        self.prev_temp_clock = read_clock
+        self.prev_temp_deriv = temp_deriv
+        if co == bounded_co:
+            self.prev_temp_integ = temp_integ
+    def check_busy(self, eventtime):
+        temp_diff = self.heater.target_temp - self.heater.last_temp
+        return abs(temp_diff) > 1. or abs(self.prev_temp_deriv) > 0.1
+
+
+######################################################################
+# Ziegler-Nichols PID autotuning
+######################################################################
+
+TUNE_PID_DELTA = 5.0
+
+class ControlAutoTune:
+    def __init__(self, heater, old_control, target_temp):
+        self.heater = heater
+        self.old_control = old_control
+        self.target_temp = target_temp
+        self.heating = False
+        self.peaks = []
+        self.peak = 0.
+        self.peak_clock = 0
+    def adc_callback(self, read_clock, temp):
+        if self.heating and temp >= self.target_temp:
+            self.heating = False
+            self.check_peaks()
+        elif not self.heating and temp <= self.target_temp - TUNE_PID_DELTA:
+            self.heating = True
+            self.check_peaks()
+        if self.heating:
+            self.heater.set_pwm(read_clock, PWM_MAX)
+            if temp < self.peak:
+                self.peak = temp
+                self.peak_clock = read_clock
+        else:
+            self.heater.set_pwm(read_clock, 0)
+            if temp > self.peak:
+                self.peak = temp
+                self.peak_clock = read_clock
+    def check_peaks(self):
+        self.peaks.append((self.peak, self.peak_clock))
+        if self.heating:
+            self.peak = 9999999.
+        else:
+            self.peak = -9999999.
+        if len(self.peaks) < 4:
+            return
+        temp_diff = self.peaks[-1][0] - self.peaks[-2][0]
+        clock_diff = self.peaks[-1][1] - self.peaks[-3][1]
+        pwm_diff = PWM_MAX - 0
+        Ku = 4. * (2. * pwm_diff) / (abs(temp_diff) * math.pi)
+        Tu = clock_diff / self.heater.printer.mcu.get_mcu_freq()
+
+        Kp = 0.6 * Ku
+        Ti = 0.5 * Tu
+        Td = 0.125 * Tu
+        Ki = Kp / Ti
+        Kd = Kp * Td
+        logging.info("Autotune: raw=%f/%d/%d Ku=%f Tu=%f  Kp=%f Ki=%f Kd=%f" % (
+            temp_diff, clock_diff, pwm_diff, Ku, Tu, Kp, Ki, Kd))
+    def check_busy(self, eventtime):
+        if self.heating or len(self.peaks) < 12:
+            return True
+        self.heater.control = self.old_control
+        return False
+
+
+######################################################################
+# Tuning information test
+######################################################################
+
+class ControlBumpTest:
+    def __init__(self, heater, old_control, target_temp):
+        self.heater = heater
+        self.old_control = old_control
+        self.target_temp = target_temp
+        self.temp_samples = {}
+        self.pwm_samples = {}
+        self.state = 0
+    def set_pwm(self, read_clock, value):
+        self.pwm_samples[read_clock + 2*self.heater.report_clock] = value
+        self.heater.set_pwm(read_clock, value)
+    def adc_callback(self, read_clock, temp):
+        self.temp_samples[read_clock] = temp
+        if not self.state:
+            self.set_pwm(read_clock, 0)
+            if len(self.temp_samples) >= 20:
+                self.state += 1
+        elif self.state == 1:
+            if temp < self.target_temp:
+                self.set_pwm(read_clock, PWM_MAX)
+                return
+            self.set_pwm(read_clock, 0)
+            self.state += 1
+        elif self.state == 2:
+            self.set_pwm(read_clock, 0)
+            if temp <= (self.target_temp + AMBIENT_TEMP) / 2.:
+                self.dump_stats()
+                self.state += 1
+    def dump_stats(self):
+        out = ["%d %.1f %d" % (clock, temp, self.pwm_samples.get(clock, -1))
+               for clock, temp in sorted(self.temp_samples.items())]
+        f = open("/tmp/heattest.txt", "wb")
+        f.write('\n'.join(out))
+        f.close()
+    def check_busy(self, eventtime):
+        if self.state < 3:
+            return True
+        self.heater.control = self.old_control
+        return False
diff --git a/klippy/homing.py b/klippy/homing.py
new file mode 100644
index 00000000..566cdf95
--- /dev/null
+++ b/klippy/homing.py
@@ -0,0 +1,82 @@
+# Code for state tracking during homing operations
+#
+# Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+
+import logging
+
+class Homing:
+    def __init__(self, kin, steppers):
+        self.kin = kin
+        self.steppers = steppers
+
+        self.states = []
+        self.endstops = []
+        self.changed_axis = []
+    def plan_home(self, axis, precise=False):
+        s = self.steppers[axis]
+        state = self.states
+        self.changed_axis.append(axis)
+        speed = s.homing_speed
+        if s.homing_positive_dir:
+            pos = s.position_endstop + 1.5*(s.position_min - s.position_endstop)
+            rpos = s.position_endstop - s.homing_retract_dist
+            r2pos = rpos - s.homing_retract_dist
+        else:
+            pos = s.position_endstop + 1.5*(s.position_max - s.position_endstop)
+            rpos = s.position_endstop + s.homing_retract_dist
+            r2pos = rpos + s.homing_retract_dist
+        # Initial homing
+        state.append((self.do_home, ({axis: pos}, speed)))
+        state.append((self.do_wait_endstop, ({axis: 1},)))
+        # Retract
+        state.append((self.do_move, ({axis: rpos}, speed)))
+        # Home again
+        state.append((self.do_home, ({axis: r2pos}, speed/2.0)))
+        state.append((self.do_wait_endstop, ({axis: 1},)))
+    def plan_axis_update(self, callback):
+        self.states.append((callback, (self.changed_axis,)))
+    def check_busy(self, eventtime):
+        while self.states:
+            first = self.states[0]
+            ret = first[0](*first[1])
+            if ret:
+                return True
+            self.states.pop(0)
+        return False
+
+    def do_move(self, axis_pos, speed):
+        # Issue a move command to axis_pos
+        newpos = self.kin.get_position()
+        for axis, pos in axis_pos.items():
+            newpos[axis] = pos
+        self.kin.move(newpos, speed)
+        return False
+    def do_home(self, axis_pos, speed):
+        # Alter kinematics class to think printer is at axis_pos
+        newpos = self.kin.get_position()
+        forcepos = list(newpos)
+        for axis, pos in axis_pos.items():
+            newpos[axis] = self.steppers[axis].position_endstop
+            forcepos[axis] = pos
+        self.kin.set_position(forcepos)
+        # Start homing and issue move
+        print_time = self.kin.get_last_move_time()
+        for axis in axis_pos:
+            hz = speed * self.steppers[axis].inv_step_dist
+            es = self.steppers[axis].enable_endstop_checking(print_time, hz)
+            self.endstops.append(es)
+        self.kin.move(newpos, speed)
+        self.kin.reset_print_time()
+        for es in self.endstops:
+            es.home_finalize()
+        return False
+    def do_wait_endstop(self, axis_wait):
+        # Check if axis_wait endstops have triggered
+        for es in self.endstops:
+            if es.is_homing():
+                return True
+        # Finished
+        del self.endstops[:]
+        return False
diff --git a/klippy/klippy.py b/klippy/klippy.py
new file mode 100644
index 00000000..143dd762
--- /dev/null
+++ b/klippy/klippy.py
@@ -0,0 +1,163 @@
+#!/usr/bin/env python
+# Main code for host side printer firmware
+#
+# Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import sys, optparse, ConfigParser, logging, time, threading
+import gcode, cartesian, util, mcu, fan, heater, reactor
+
+class ConfigWrapper:
+    def __init__(self, printer, section):
+        self.printer = printer
+        self.section = section
+    def get(self, option, default=None):
+        if not self.printer.fileconfig.has_option(self.section, option):
+            return default
+        return self.printer.fileconfig.get(self.section, option)
+    def getint(self, option, default=None):
+        if not self.printer.fileconfig.has_option(self.section, option):
+            return default
+        return self.printer.fileconfig.getint(self.section, option)
+    def getfloat(self, option, default=None):
+        if not self.printer.fileconfig.has_option(self.section, option):
+            return default
+        return self.printer.fileconfig.getfloat(self.section, option)
+    def getboolean(self, option, default=None):
+        if not self.printer.fileconfig.has_option(self.section, option):
+            return default
+        return self.printer.fileconfig.getboolean(self.section, option)
+    def getsection(self, section):
+        return ConfigWrapper(self.printer, section)
+
+class Printer:
+    def __init__(self, conffile, debuginput=None):
+        self.fileconfig = ConfigParser.RawConfigParser()
+        self.fileconfig.read(conffile)
+        self.reactor = reactor.Reactor()
+
+        self._pconfig = ConfigWrapper(self, 'printer')
+        ptty = self._pconfig.get('pseudo_tty', '/tmp/printer')
+        if debuginput is None:
+            pseudo_tty = util.create_pty(ptty)
+        else:
+            pseudo_tty = debuginput.fileno()
+
+        self.gcode = gcode.GCodeParser(
+            self, pseudo_tty, inputfile=debuginput is not None)
+        self.mcu = None
+        self.stat_timer = None
+
+        self.objects = {}
+        if self.fileconfig.has_section('fan'):
+            self.objects['fan'] = fan.PrinterFan(
+                self, ConfigWrapper(self, 'fan'))
+        if self.fileconfig.has_section('heater_nozzle'):
+            self.objects['heater_nozzle'] = heater.PrinterHeater(
+                self, ConfigWrapper(self, 'heater_nozzle'))
+        if self.fileconfig.has_section('heater_bed'):
+            self.objects['heater_bed'] = heater.PrinterHeater(
+                self, ConfigWrapper(self, 'heater_bed'))
+        self.objects['kinematics'] = cartesian.CartKinematics(
+            self, self._pconfig)
+
+    def stats(self, eventtime):
+        out = []
+        out.append(self.gcode.stats(eventtime))
+        out.append(self.objects['kinematics'].stats(eventtime))
+        out.append(self.mcu.stats(eventtime))
+        logging.info("Stats %.0f: %s" % (eventtime, ' '.join(out)))
+        return eventtime + 1.
+    def build_config(self):
+        for oname in sorted(self.objects.keys()):
+            self.objects[oname].build_config()
+        self.gcode.build_config()
+        self.mcu.build_config()
+    def connect(self):
+        self.mcu = mcu.MCU(self, ConfigWrapper(self, 'mcu'))
+        self.mcu.connect()
+        self.build_config()
+        self.stats_timer = self.reactor.register_timer(
+            self.stats, self.reactor.NOW)
+    def connect_debug(self, debugoutput):
+        self.mcu = mcu.DummyMCU(debugoutput)
+        self.mcu.connect()
+        self.build_config()
+    def connect_file(self, output, dictionary):
+        self.mcu = mcu.MCU(self, ConfigWrapper(self, 'mcu'))
+        self.mcu.connect_file(output, dictionary)
+        self.build_config()
+    def run(self):
+        self.gcode.run()
+        # If gcode exits, then exit the MCU
+        self.stats(time.time())
+        self.mcu.disconnect()
+        self.stats(time.time())
+    def shutdown(self):
+        self.gcode.shutdown()
+
+
+######################################################################
+# Startup
+######################################################################
+
+def read_dictionary(filename):
+    dfile = open(filename, 'rb')
+    dictionary = dfile.read()
+    dfile.close()
+    return dictionary
+
+def store_dictionary(filename, printer):
+    f = open(filename, 'wb')
+    f.write(printer.mcu.serial.msgparser.raw_identify_data)
+    f.close()
+
+def main():
+    usage = "%prog [options] <config file>"
+    opts = optparse.OptionParser(usage)
+    opts.add_option("-o", "--debugoutput", dest="outputfile",
+                    help="write output to file instead of to serial port")
+    opts.add_option("-i", "--debuginput", dest="inputfile",
+                    help="read commands from file instead of from tty port")
+    opts.add_option("-l", "--logfile", dest="logfile",
+                    help="write log to file instead of stderr")
+    opts.add_option("-v", action="store_true", dest="verbose",
+                    help="enable debug messages")
+    opts.add_option("-d", dest="read_dictionary",
+                    help="file to read for mcu protocol dictionary")
+    opts.add_option("-D", dest="write_dictionary",
+                    help="file to write mcu protocol dictionary")
+    options, args = opts.parse_args()
+    if len(args) != 1:
+        opts.error("Incorrect number of arguments")
+    conffile = args[0]
+
+    debuginput = debugoutput = None
+
+    debuglevel = logging.INFO
+    if options.verbose:
+        debuglevel = logging.DEBUG
+    if options.inputfile:
+        debuginput = open(options.inputfile, 'rb')
+    if options.outputfile:
+        debugoutput = open(options.outputfile, 'wb')
+    if options.logfile:
+        logoutput = open(options.logfile, 'wb')
+        logging.basicConfig(stream=logoutput, level=debuglevel)
+    else:
+        logging.basicConfig(level=debuglevel)
+    logging.info("Starting Klippy...")
+
+    # Start firmware
+    printer = Printer(conffile, debuginput=debuginput)
+    if debugoutput:
+        proto_dict = read_dictionary(options.read_dictionary)
+        printer.connect_file(debugoutput, proto_dict)
+    else:
+        printer.connect()
+    if options.write_dictionary:
+        store_dictionary(options.write_dictionary, printer)
+    printer.run()
+
+if __name__ == '__main__':
+    main()
diff --git a/klippy/list.h b/klippy/list.h
new file mode 100644
index 00000000..317a109c
--- /dev/null
+++ b/klippy/list.h
@@ -0,0 +1,108 @@
+#ifndef __LIST_H
+#define __LIST_H
+
+#define container_of(ptr, type, member) ({                      \
+        const typeof( ((type *)0)->member ) *__mptr = (ptr);    \
+        (type *)( (char *)__mptr - offsetof(type,member) );})
+
+
+/****************************************************************
+ * list - Double linked lists
+ ****************************************************************/
+
+struct list_node {
+    struct list_node *next, *prev;
+};
+
+struct list_head {
+    struct list_node root;
+};
+
+static inline void
+list_init(struct list_head *h)
+{
+    h->root.prev = h->root.next = &h->root;
+}
+
+static inline int
+list_empty(const struct list_head *h)
+{
+    return h->root.next == &h->root;
+}
+
+static inline void
+list_del(struct list_node *n)
+{
+    struct list_node *prev = n->prev;
+    struct list_node *next = n->next;
+    next->prev = prev;
+    prev->next = next;
+}
+
+static inline void
+__list_add(struct list_node *n, struct list_node *prev, struct list_node *next)
+{
+    next->prev = n;
+    n->next = next;
+    n->prev = prev;
+    prev->next = n;
+}
+
+static inline void
+list_add_after(struct list_node *n, struct list_node *prev)
+{
+    __list_add(n, prev, prev->next);
+}
+
+static inline void
+list_add_before(struct list_node *n, struct list_node *next)
+{
+    __list_add(n, next->prev, next);
+}
+
+static inline void
+list_add_head(struct list_node *n, struct list_head *h)
+{
+    list_add_after(n, &h->root);
+}
+
+static inline void
+list_add_tail(struct list_node *n, struct list_head *h)
+{
+    list_add_before(n, &h->root);
+}
+
+static inline void
+list_join_tail(struct list_head *add, struct list_head *h)
+{
+    if (!list_empty(add)) {
+        struct list_node *prev = h->root.prev;
+        struct list_node *next = &h->root;
+        struct list_node *first = add->root.next;
+        struct list_node *last = add->root.prev;
+        first->prev = prev;
+        prev->next = first;
+        last->next = next;
+        next->prev = last;
+    }
+}
+
+#define list_next_entry(pos, member)                            \
+    container_of((pos)->member.next, typeof(*pos), member)
+
+#define list_first_entry(head, type, member)                    \
+    container_of((head)->root.next, type, member)
+
+#define list_for_each_entry(pos, head, member)                  \
+    for (pos = list_first_entry((head), typeof(*pos), member)   \
+         ; &pos->member != &(head)->root                        \
+         ; pos = list_next_entry(pos, member))
+
+#define list_for_each_entry_safe(pos, n, head, member)          \
+    for (pos = list_first_entry((head), typeof(*pos), member)   \
+          , n = list_next_entry(pos, member)                    \
+         ; &pos->member != &(head)->root                        \
+         ; pos = n, n = list_next_entry(n, member))
+
+
+#endif // list.h
diff --git a/klippy/lookahead.py b/klippy/lookahead.py
new file mode 100644
index 00000000..9e377463
--- /dev/null
+++ b/klippy/lookahead.py
@@ -0,0 +1,50 @@
+# Move queue look-ahead
+#
+# Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+
+class MoveQueue:
+    def __init__(self, dummy_move):
+        self.dummy_move = dummy_move
+        self.queue = []
+        self.prev_junction_max = 0.
+        self.junction_flush = 0.
+    def prev_move(self):
+        if self.queue:
+            return self.queue[-1]
+        return self.dummy_move
+    def flush(self, lazy=False):
+        next_junction_max = 0.
+        can_flush = not lazy
+        flush_count = len(self.queue)
+        junction_end = [None] * flush_count
+        for i in range(len(self.queue)-1, -1, -1):
+            move = self.queue[i]
+            junction_end[i] = next_junction_max
+            if not can_flush:
+                flush_count -= 1
+            next_junction_max = next_junction_max + move.junction_delta
+            if next_junction_max >= move.junction_start_max:
+                next_junction_max = move.junction_start_max
+                can_flush = True
+        prev_junction_max = self.prev_junction_max
+        for i in range(flush_count):
+            move = self.queue[i]
+            next_junction_max = min(prev_junction_max + move.junction_delta
+                                    , junction_end[i])
+            move.process(prev_junction_max, next_junction_max)
+            prev_junction_max = next_junction_max
+        del self.queue[:flush_count]
+        self.prev_junction_max = prev_junction_max
+        self.junction_flush = 0.
+        if self.queue:
+            self.junction_flush = self.queue[-1].junction_max
+    def add_move(self, move):
+        self.queue.append(move)
+        if len(self.queue) == 1:
+            self.junction_flush = move.junction_max
+            return
+        self.junction_flush -= move.junction_delta
+        if self.junction_flush <= 0.:
+            self.flush(lazy=True)
diff --git a/klippy/mcu.py b/klippy/mcu.py
new file mode 100644
index 00000000..728ec91d
--- /dev/null
+++ b/klippy/mcu.py
@@ -0,0 +1,510 @@
+# Multi-processor safe interface to micro-controller
+#
+# Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import sys, zlib, logging, time, math
+import serialhdl, pins, chelper
+
+def parse_pin_extras(pin, can_pullup=False):
+    pullup = invert = 0
+    if can_pullup and pin.startswith('^'):
+        pullup = invert = 1
+        pin = pin[1:].strip()
+    if pin.startswith('!'):
+        invert = invert ^ 1
+        pin = pin[1:].strip()
+    return pin, pullup, invert
+
+class MCU_stepper:
+    def __init__(self, mcu, step_pin, dir_pin, min_stop_interval, max_error):
+        self._mcu = mcu
+        self._oid = mcu.create_oid()
+        step_pin, pullup, invert_step = parse_pin_extras(step_pin)
+        dir_pin, pullup, self._invert_dir = parse_pin_extras(dir_pin)
+        self._sdir = -1
+        self._last_move_clock = -2**29
+        mcu.add_config_cmd(
+            "config_stepper oid=%d step_pin=%s dir_pin=%s"
+            " min_stop_interval=%d invert_step=%d" % (
+                self._oid, step_pin, dir_pin, min_stop_interval, invert_step))
+        mcu.register_stepper(self)
+        self._step_cmd = mcu.lookup_command(
+            "queue_step oid=%c interval=%u count=%hu add=%hi")
+        self._dir_cmd = mcu.lookup_command(
+            "set_next_step_dir oid=%c dir=%c")
+        self._reset_cmd = mcu.lookup_command(
+            "reset_step_clock oid=%c clock=%u")
+        ffi_main, self.ffi_lib = chelper.get_ffi()
+        self._stepqueue = self.ffi_lib.stepcompress_alloc(
+            max_error, self._step_cmd.msgid, self._oid)
+    def get_oid(self):
+        return self._oid
+    def note_stepper_stop(self):
+        self._sdir = -1
+        self._last_move_clock = -2**29
+    def reset_step_clock(self, clock):
+        self.ffi_lib.stepcompress_reset(self._stepqueue, clock)
+        data = (self._reset_cmd.msgid, self._oid, clock & 0xffffffff)
+        self.ffi_lib.stepcompress_queue_msg(self._stepqueue, data, len(data))
+    def set_next_step_dir(self, sdir, clock):
+        if clock - self._last_move_clock >= 2**29:
+            self.reset_step_clock(clock)
+        self._last_move_clock = clock
+        if self._sdir == sdir:
+            return
+        self._sdir = sdir
+        data = (self._dir_cmd.msgid, self._oid, sdir ^ self._invert_dir)
+        self.ffi_lib.stepcompress_queue_msg(self._stepqueue, data, len(data))
+    def step(self, steptime):
+        self.ffi_lib.stepcompress_push(self._stepqueue, steptime)
+    def step_sqrt(self, steps, step_offset, clock_offset, sqrt_offset, factor):
+        return self.ffi_lib.stepcompress_push_sqrt(
+            self._stepqueue, steps, step_offset, clock_offset
+            , sqrt_offset, factor)
+    def step_factor(self, steps, step_offset, clock_offset, factor):
+        return self.ffi_lib.stepcompress_push_factor(
+            self._stepqueue, steps, step_offset, clock_offset, factor)
+    def get_errors(self):
+        return self.ffi_lib.stepcompress_get_errors(self._stepqueue)
+    def get_print_clock(self, print_time):
+        return self._mcu.get_print_clock(print_time)
+
+class MCU_endstop:
+    RETRY_QUERY = 1.000
+    def __init__(self, mcu, pin, stepper):
+        self._mcu = mcu
+        self._oid = mcu.create_oid()
+        self._stepper = stepper
+        stepper_oid = stepper.get_oid()
+        pin, pullup, self._invert = parse_pin_extras(pin, can_pullup=True)
+        self._cmd_queue = mcu.alloc_command_queue()
+        mcu.add_config_cmd(
+            "config_end_stop oid=%d pin=%s pull_up=%d stepper_oid=%d" % (
+                self._oid, pin, pullup, stepper_oid))
+        self._home_cmd = mcu.lookup_command(
+            "end_stop_home oid=%c clock=%u rest_ticks=%u pin_value=%c")
+        mcu.register_msg(self._handle_end_stop_state, "end_stop_state"
+                         , self._oid)
+        self._query_cmd = mcu.lookup_command("end_stop_query oid=%c")
+        self._homing = False
+        self._next_query_clock = 0
+        mcu_freq = self._mcu.get_mcu_freq()
+        self._retry_query_ticks = mcu_freq * self.RETRY_QUERY
+    def home(self, clock, rest_ticks):
+        self._homing = True
+        self._next_query_clock = clock + self._retry_query_ticks
+        msg = self._home_cmd.encode(
+            self._oid, clock, rest_ticks, 1 ^ self._invert)
+        self._mcu.send(msg, reqclock=clock, cq=self._cmd_queue)
+    def home_finalize(self):
+        # XXX - this flushes the serial port of messages ready to be
+        # sent, but doesn't flush messages if they had an unmet minclock
+        self._mcu.serial.send_flush()
+        self._stepper.note_stepper_stop()
+    def _handle_end_stop_state(self, params):
+        logging.debug("end_stop_state %s" % (params,))
+        self._homing = params['homing'] != 0
+    def is_homing(self):
+        if not self._homing:
+            return self._homing
+        if self._mcu.output_file_mode:
+            return False
+        last_clock = self._mcu.get_last_clock()
+        if last_clock >= self._next_query_clock:
+            self._next_query_clock = last_clock + self._retry_query_ticks
+            msg = self._query_cmd.encode(self._oid)
+            self._mcu.send(msg, cq=self._cmd_queue)
+        return self._homing
+    def get_print_clock(self, print_time):
+        return self._mcu.get_print_clock(print_time)
+
+class MCU_digital_out:
+    def __init__(self, mcu, pin, max_duration):
+        self._mcu = mcu
+        self._oid = mcu.create_oid()
+        pin, pullup, self._invert = parse_pin_extras(pin)
+        self._last_clock = 0
+        self._last_value = None
+        self._cmd_queue = mcu.alloc_command_queue()
+        mcu.add_config_cmd(
+            "config_digital_out oid=%d pin=%s default_value=%d"
+            " max_duration=%d" % (self._oid, pin, self._invert, max_duration))
+        self._set_cmd = mcu.lookup_command(
+            "schedule_digital_out oid=%c clock=%u value=%c")
+    def set_digital(self, clock, value):
+        msg = self._set_cmd.encode(self._oid, clock, value ^ self._invert)
+        self._mcu.send(msg, minclock=self._last_clock, reqclock=clock
+                      , cq=self._cmd_queue)
+        self._last_clock = clock
+        self._last_value = value
+    def get_last_setting(self):
+        return self._last_value
+    def set_pwm(self, clock, value):
+        dval = 0
+        if value > 127:
+            dval = 1
+        self.set_digital(clock, dval)
+    def get_print_clock(self, print_time):
+        return self._mcu.get_print_clock(print_time)
+
+class MCU_pwm:
+    def __init__(self, mcu, pin, cycle_ticks, max_duration, hard_pwm=True):
+        self._mcu = mcu
+        self._oid = mcu.create_oid()
+        self._last_clock = 0
+        self._cmd_queue = mcu.alloc_command_queue()
+        if hard_pwm:
+            mcu.add_config_cmd(
+                "config_pwm_out oid=%d pin=%s cycle_ticks=%d default_value=0"
+                " max_duration=%d" % (self._oid, pin, cycle_ticks, max_duration))
+            self._set_cmd = mcu.lookup_command(
+                "schedule_pwm_out oid=%c clock=%u value=%c")
+        else:
+            mcu.add_config_cmd(
+                "config_soft_pwm_out oid=%d pin=%s cycle_ticks=%d"
+                " default_value=0 max_duration=%d" % (
+                    self._oid, pin, cycle_ticks, max_duration))
+            self._set_cmd = mcu.lookup_command(
+                "schedule_soft_pwm_out oid=%c clock=%u value=%c")
+    def set_pwm(self, clock, value):
+        msg = self._set_cmd.encode(self._oid, clock, value)
+        self._mcu.send(msg, minclock=self._last_clock, reqclock=clock
+                      , cq=self._cmd_queue)
+        self._last_clock = clock
+    def get_print_clock(self, print_time):
+        return self._mcu.get_print_clock(print_time)
+
+class MCU_adc:
+    ADC_MAX = 1024 # 10bit adc
+    def __init__(self, mcu, pin):
+        self._mcu = mcu
+        self._oid = mcu.create_oid()
+        self._min_sample = 0
+        self._max_sample = 0xffff
+        self._sample_ticks = 0
+        self._sample_count = 1
+        self._report_clock = 0
+        self._last_value = 0
+        self._last_read_clock = 0
+        self._callback = None
+        self._max_adc_inv = 0.
+        self._cmd_queue = mcu.alloc_command_queue()
+        mcu.add_config_cmd("config_analog_in oid=%d pin=%s" % (self._oid, pin))
+        mcu.register_msg(self._handle_analog_in_state, "analog_in_state"
+                         , self._oid)
+        self._query_cmd = mcu.lookup_command(
+            "query_analog_in oid=%c clock=%u sample_ticks=%u sample_count=%c"
+            " rest_ticks=%u min_value=%hu max_value=%hu")
+    def set_minmax(self, sample_ticks, sample_count, minval=None, maxval=None):
+        if minval is None:
+            minval = 0
+        if maxval is None:
+            maxval = 0xffff
+        self._sample_ticks = sample_ticks
+        self._sample_count = sample_count
+        max_adc = sample_count * self.ADC_MAX
+        self._min_sample = int(minval * max_adc)
+        self._max_sample = min(0xffff, int(math.ceil(maxval * max_adc)))
+        self._max_adc_inv = 1.0 / max_adc
+    def query_analog_in(self, report_clock):
+        self._report_clock = report_clock
+        mcu_freq = self._mcu.get_mcu_freq()
+        cur_clock = self._mcu.get_last_clock()
+        clock = cur_clock + int(mcu_freq * (1.0 + self._oid * 0.01)) # XXX
+        msg = self._query_cmd.encode(
+            self._oid, clock, self._sample_ticks, self._sample_count
+            , report_clock, self._min_sample, self._max_sample)
+        self._mcu.send(msg, reqclock=clock, cq=self._cmd_queue)
+    def _handle_analog_in_state(self, params):
+        self._last_value = params['value'] * self._max_adc_inv
+        next_clock = self._mcu.serial.translate_clock(params['next_clock'])
+        self._last_read_clock = next_clock - self._report_clock
+        if self._callback is not None:
+            self._callback(self._last_read_clock, self._last_value)
+    def set_adc_callback(self, cb):
+        self._callback = cb
+    def get_print_clock(self, print_time):
+        return self._mcu.get_print_clock(print_time)
+
+class MCU:
+    def __init__(self, printer, config):
+        self._printer = printer
+        self._config = config
+        # Serial port
+        baud = config.getint('baud', 115200)
+        serialport = config.get('serial', '/dev/ttyS0')
+        self.serial = serialhdl.SerialReader(printer.reactor, serialport, baud)
+        self.is_shutdown = False
+        self.output_file_mode = False
+        # Config building
+        self._num_oids = 0
+        self._config_cmds = []
+        self._config_crc = None
+        # Move command queuing
+        ffi_main, self.ffi_lib = chelper.get_ffi()
+        self._steppers = []
+        self._steppersync = None
+        # Print time to clock epoch calculations
+        self._print_start_clock = 0.
+        self._clock_freq = 0.
+        # Stats
+        self._mcu_tick_avg = 0.
+        self._mcu_tick_stddev = 0.
+    def handle_mcu_stats(self, params):
+        logging.debug("mcu stats: %s" % (params,))
+        count = params['count']
+        tick_sum = params['sum']
+        c = 1.0 / (count * self._clock_freq)
+        self._mcu_tick_avg = tick_sum * c
+        tick_sumsq = params['sumsq']
+        tick_sumavgsq = ((tick_sum // (256*count)) * count)**2
+        self._mcu_tick_stddev = c * 256. * math.sqrt(
+            count * tick_sumsq - tick_sumavgsq)
+    def handle_shutdown(self, params):
+        if self.is_shutdown:
+            return
+        self.is_shutdown = True
+        logging.info("%s: %s" % (params['#name'], params['#msg']))
+        self.serial.dump_debug()
+        self._printer.shutdown()
+    # Connection phase
+    def _init_steppersync(self, count):
+        stepqueues = tuple(s._stepqueue for s in self._steppers)
+        self._steppersync = self.ffi_lib.steppersync_alloc(
+            self.serial.serialqueue, stepqueues, len(stepqueues), count)
+    def connect(self):
+        def handle_serial_state(params):
+            if params['#state'] == 'connected':
+                self._printer.reactor.end()
+        self.serial.register_callback(handle_serial_state, '#state')
+        self.serial.connect()
+        self._printer.reactor.run()
+        self.serial.unregister_callback('#state')
+        logging.info("serial connected")
+        self._clock_freq = float(self.serial.msgparser.config['CLOCK_FREQ'])
+        self.register_msg(self.handle_shutdown, 'shutdown')
+        self.register_msg(self.handle_shutdown, 'is_shutdown')
+        self.register_msg(self.handle_mcu_stats, 'stats')
+    def connect_file(self, debugoutput, dictionary, pace=False):
+        self.output_file_mode = True
+        self.serial.connect_file(debugoutput, dictionary)
+        self._clock_freq = float(self.serial.msgparser.config['CLOCK_FREQ'])
+        def dummy_build_config():
+            self._init_steppersync(500)
+        self.build_config = dummy_build_config
+        if not pace:
+            def dummy_set_print_start_time(eventtime):
+                pass
+            def dummy_get_print_buffer_time(eventtime, last_move_end):
+                return 0.250
+            self.set_print_start_time = dummy_set_print_start_time
+            self.get_print_buffer_time = dummy_get_print_buffer_time
+    def disconnect(self):
+        self.serial.disconnect()
+    def stats(self, eventtime):
+        stats = self.serial.stats(eventtime)
+        stats += " mcu_task_avg=%.06f mcu_task_stddev=%.06f" % (
+            self._mcu_tick_avg, self._mcu_tick_stddev)
+        err = 0
+        for s in self._steppers:
+            err += s.get_errors()
+        if err:
+            stats += " step_errors=%d" % (err,)
+        return stats
+    # Configuration phase
+    def _add_custom(self):
+        data = self._config.get('custom', '')
+        for line in data.split('\n'):
+            line = line.strip()
+            cpos = line.find('#')
+            if cpos >= 0:
+                line = line[:cpos].strip()
+            if not line:
+                continue
+            self.add_config_cmd(line)
+    def build_config(self):
+        # Build config commands
+        self._add_custom()
+        self._config_cmds.insert(0, "allocate_oids count=%d" % (
+            self._num_oids,))
+
+        # Resolve pin names
+        mcu = self.serial.msgparser.config['MCU']
+        pin_map = self._config.get('pin_map')
+        if pin_map is None:
+            pnames = pins.mcu_to_pins(mcu)
+        else:
+            pnames = pins.map_pins(pin_map, mcu)
+        self._config_cmds = [pins.update_command(c, pnames)
+                             for c in self._config_cmds]
+
+        # Calculate config CRC
+        self._config_crc = zlib.crc32('\n'.join(self._config_cmds)) & 0xffffffff
+        self.add_config_cmd("finalize_config crc=%d" % (self._config_crc,))
+
+        self._send_config()
+    def _send_config(self):
+        msg = self.create_command("get_config")
+        config_params = {}
+        sent_config = False
+        def handle_get_config(params):
+            config_params.update(params)
+            done = not sent_config or params['is_config']
+            if done:
+                self._printer.reactor.end()
+            return done
+        while 1:
+            self.serial.send_with_response(msg, handle_get_config, 'config')
+            self._printer.reactor.run()
+            if not config_params['is_config']:
+                # Send config commands
+                for c in self._config_cmds:
+                    self.send(self.create_command(c))
+                config_params.clear()
+                sent_config = True
+                continue
+            if self._config_crc != config_params['crc']:
+                logging.error("Printer CRC does not match config")
+                sys.exit(1)
+            break
+        logging.info("Configured")
+        self._init_steppersync(config_params['move_count'])
+    # Config creation helpers
+    def create_oid(self):
+        oid = self._num_oids
+        self._num_oids += 1
+        return oid
+    def add_config_cmd(self, cmd):
+        self._config_cmds.append(cmd)
+    def register_msg(self, cb, msg, oid=None):
+        self.serial.register_callback(cb, msg, oid)
+    def register_stepper(self, stepper):
+        self._steppers.append(stepper)
+    def alloc_command_queue(self):
+        return self.serial.alloc_command_queue()
+    def lookup_command(self, msgformat):
+        return self.serial.msgparser.lookup_command(msgformat)
+    def create_command(self, msg):
+        return self.serial.msgparser.create_command(msg)
+    # Wrappers for mcu object creation
+    def create_stepper(self, step_pin, dir_pin, min_stop_interval, max_error):
+        return MCU_stepper(self, step_pin, dir_pin, min_stop_interval, max_error)
+    def create_endstop(self, pin, stepper):
+        return MCU_endstop(self, pin, stepper)
+    def create_digital_out(self, pin, max_duration=2.):
+        max_duration = int(max_duration * self._clock_freq)
+        return MCU_digital_out(self, pin, max_duration)
+    def create_pwm(self, pin, hard_cycle_ticks, max_duration=2.):
+        max_duration = int(max_duration * self._clock_freq)
+        if hard_cycle_ticks:
+            return MCU_pwm(self, pin, hard_cycle_ticks, max_duration)
+        if hard_cycle_ticks < 0:
+            return MCU_digital_out(self, pin, max_duration)
+        cycle_ticks = int(self._clock_freq / 10.)
+        return MCU_pwm(self, pin, cycle_ticks, max_duration, hard_pwm=False)
+    def create_adc(self, pin):
+        return MCU_adc(self, pin)
+    # Clock syncing
+    def set_print_start_time(self, eventtime):
+        self._print_start_clock = self.serial.get_clock(eventtime)
+    def get_print_buffer_time(self, eventtime, last_move_end):
+        clock_diff = self.serial.get_clock(eventtime) - self._print_start_clock
+        return last_move_end - (float(clock_diff) / self._clock_freq)
+    def get_print_clock(self, print_time):
+        return print_time * self._clock_freq + self._print_start_clock
+    def get_mcu_freq(self):
+        return self._clock_freq
+    def get_last_clock(self):
+        return self.serial.get_last_clock()
+    # Move command queuing
+    def send(self, cmd, minclock=0, reqclock=0, cq=None):
+        self.serial.send(cmd, minclock, reqclock, cq=cq)
+    def flush_moves(self, print_time):
+        move_clock = int(self.get_print_clock(print_time))
+        self.ffi_lib.steppersync_flush(self._steppersync, move_clock)
+
+
+######################################################################
+# MCU Unit testing
+######################################################################
+
+class Dummy_MCU_stepper:
+    def __init__(self, mcu, stepid):
+        self._mcu = mcu
+        self._stepid = stepid
+        self._sdir = None
+    def queue_step(self, interval, count, add, clock):
+        dirstr = countstr = addstr = ""
+        if self._sdir is not None:
+            dirstr = "D%d" % (self._sdir+1,)
+            self._sdir = None
+        if count != 1:
+            countstr = "C%d" % (count,)
+        if add:
+            addstr = "A%d" % (add,)
+        self._mcu.outfile.write("G5S%d%s%s%sT%d\n" % (
+            self._stepid, dirstr, countstr, addstr, interval))
+    def set_next_step_dir(self, dir):
+        self._sdir = dir
+    def reset_step_clock(self, clock):
+        self._mcu.outfile.write("G6S%dT%d\n" % (self._stepid, clock))
+    def get_print_clock(self, print_time):
+        return self._mcu.get_print_clock(print_time)
+
+class Dummy_MCU_obj:
+    def __init__(self, mcu):
+        self._mcu = mcu
+    def home(self, clock, rest_ticks):
+        pass
+    def is_homing(self):
+        return False
+    def home_finalize(self):
+        pass
+    def set_pwm(self, print_time, value):
+        pass
+    def set_minmax(self, sample_ticks, sample_count, minval=None, maxval=None):
+        pass
+    def query_analog_in(self, report_clock):
+        pass
+    def set_adc_callback(self, cb):
+        pass
+    def get_print_clock(self, print_time):
+        return self._mcu.get_print_clock(print_time)
+
+class DummyMCU:
+    def __init__(self, outfile):
+        self.outfile = outfile
+        self._stepid = -1
+        self._print_start_clock = 0.
+        self._clock_freq = 16000000.
+        logging.debug('Translated by klippy')
+    def connect(self):
+        pass
+    def disconnect(self):
+        pass
+    def stats(self, eventtime):
+        return ""
+    def build_config(self):
+        pass
+    def create_stepper(self, step_pin, dir_pin, min_stop_interval, max_error):
+        self._stepid += 1
+        return Dummy_MCU_stepper(self, self._stepid)
+    def create_endstop(self, pin, stepper):
+        return Dummy_MCU_obj(self)
+    def create_digital_out(self, pin, max_duration=2.):
+        return None
+    def create_pwm(self, pin, hard_cycle_ticks, max_duration=2.):
+        return Dummy_MCU_obj(self)
+    def create_adc(self, pin):
+        return Dummy_MCU_obj(self)
+    def set_print_start_time(self, eventtime):
+        pass
+    def get_print_buffer_time(self, eventtime, last_move_end):
+        return 0.250
+    def get_print_clock(self, print_time):
+        return print_time * self._clock_freq + self._print_start_clock
+    def get_mcu_freq(self):
+        return self._clock_freq
+    def flush_moves(self, print_time):
+        pass
diff --git a/klippy/msgproto.py b/klippy/msgproto.py
new file mode 100644
index 00000000..73c8d2d5
--- /dev/null
+++ b/klippy/msgproto.py
@@ -0,0 +1,313 @@
+# Protocol definitions for firmware communication
+#
+# Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import json, zlib, logging
+
+DefaultMessages = {
+    0: "identify_response offset=%u data=%.*s",
+    1: "identify offset=%u count=%c",
+}
+
+MESSAGE_MIN = 5
+MESSAGE_MAX = 64
+MESSAGE_HEADER_SIZE  = 2
+MESSAGE_TRAILER_SIZE = 3
+MESSAGE_POS_LEN = 0
+MESSAGE_POS_SEQ = 1
+MESSAGE_TRAILER_CRC  = 3
+MESSAGE_TRAILER_SYNC = 1
+MESSAGE_PAYLOAD_MAX = MESSAGE_MAX - MESSAGE_MIN
+MESSAGE_SEQ_MASK = 0x0f
+MESSAGE_DEST = 0x10
+MESSAGE_SYNC = '\x7E'
+
+class error(Exception):
+    def __init__(self, msg):
+        self.msg = msg
+    def __str__(self):
+        return self.msg
+
+def crc16_ccitt(buf):
+    crc = 0xffff
+    for data in buf:
+        data = ord(data)
+        data ^= crc & 0xff
+        data ^= (data & 0x0f) << 4
+        crc = ((data << 8) | (crc >> 8)) ^ (data >> 4) ^ (data << 3)
+    crc = chr(crc >> 8) + chr(crc & 0xff)
+    return crc
+
+class PT_uint32:
+    is_int = 1
+    max_length = 5
+    signed = 0
+    def encode(self, out, v):
+        if v >= 0xc000000 or v < -0x4000000: out.append((v>>28) & 0x7f | 0x80)
+        if v >= 0x180000 or v < -0x80000:    out.append((v>>21) & 0x7f | 0x80)
+        if v >= 0x3000 or v < -0x1000:       out.append((v>>14) & 0x7f | 0x80)
+        if v >= 0x60 or v < -0x20:           out.append((v>>7)  & 0x7f | 0x80)
+        out.append(v & 0x7f)
+    def parse(self, s, pos):
+        c = s[pos]
+        pos += 1
+        v = c & 0x7f
+        if (c & 0x60) == 0x60:
+            v |= -0x20
+        while c & 0x80:
+            c = s[pos]
+            pos += 1
+            v = (v<<7) | (c & 0x7f)
+        if not self.signed:
+            v = int(v & 0xffffffff)
+        return v, pos
+
+class PT_int32(PT_uint32):
+    signed = 1
+class PT_uint16(PT_uint32):
+    max_length = 3
+class PT_int16(PT_int32):
+    signed = 1
+    max_length = 3
+class PT_byte(PT_uint32):
+    max_length = 2
+
+class PT_string:
+    is_int = 0
+    max_length = 64
+    def encode(self, out, v):
+        out.append(len(v))
+        out.extend(bytearray(v))
+    def parse(self, s, pos):
+        l = s[pos]
+        return str(bytearray(s[pos+1:pos+l+1])), pos+l+1
+class PT_progmem_buffer(PT_string):
+    pass
+class PT_buffer(PT_string):
+    pass
+
+MessageTypes = {
+    '%u': PT_uint32(), '%i': PT_int32(),
+    '%hu': PT_uint16(), '%hi': PT_int16(),
+    '%c': PT_byte(),
+    '%s': PT_string(), '%.*s': PT_progmem_buffer(), '%*s': PT_buffer(),
+}
+
+# Update the message format to be compatible with python's % operator
+def convert_msg_format(msgformat):
+    mf = msgformat.replace('%c', '%u')
+    mf = mf.replace('%.*s', '%s').replace('%*s', '%s')
+    return mf
+
+class MessageFormat:
+    def __init__(self, msgid, msgformat):
+        self.msgid = msgid
+        self.msgformat = msgformat
+        self.debugformat = convert_msg_format(msgformat)
+        parts = msgformat.split()
+        self.name = parts[0]
+        argparts = [arg.split('=') for arg in parts[1:]]
+        self.param_types = [MessageTypes[fmt] for name, fmt in argparts]
+        self.param_names = [name for name, fmt in argparts]
+        self.name_to_type = dict(zip(self.param_names, self.param_types))
+    def encode(self, *params):
+        out = []
+        out.append(self.msgid)
+        for i, t in enumerate(self.param_types):
+            t.encode(out, params[i])
+        return out
+    def encode_by_name(self, **params):
+        out = []
+        out.append(self.msgid)
+        for name, t in zip(self.param_names, self.param_types):
+            t.encode(out, params[name])
+        return out
+    def parse(self, s, pos):
+        pos += 1
+        out = {}
+        for t, name in zip(self.param_types, self.param_names):
+            v, pos = t.parse(s, pos)
+            out[name] = v
+        return out, pos
+    def dump(self, s, pos):
+        pos += 1
+        out = []
+        for t in self.param_types:
+            v, pos = t.parse(s, pos)
+            if not t.is_int:
+                v = repr(v)
+            out.append(v)
+        outmsg = self.debugformat % tuple(out)
+        return outmsg, pos
+
+class OutputFormat:
+    name = '#output'
+    def __init__(self, msgid, msgformat):
+        self.msgid = msgid
+        self.msgformat = msgformat
+        self.debugformat = convert_msg_format(msgformat)
+        self.param_types = []
+        args = msgformat
+        while 1:
+            pos = args.find('%')
+            if pos < 0:
+                break
+            if pos+1 >= len(args) or args[pos+1] != '%':
+                for i in range(4):
+                    t = MessageTypes.get(args[pos:pos+1+i])
+                    if t is not None:
+                        self.param_types.append(t)
+                        break
+                else:
+                    raise error("Invalid output format for '%s'" % (msg,))
+            args = args[pos+1:]
+    def parse(self, s, pos):
+        pos += 1
+        out = []
+        for t in self.param_types:
+            v, pos = t.parse(s, pos)
+            out.append(v)
+        outmsg = self.debugformat % tuple(out)
+        return {'#msg': outmsg}, pos
+    def dump(self, s, pos):
+        pos += 1
+        out = []
+        for t in self.param_types:
+            v, pos = t.parse(s, pos)
+            out.append(v)
+        outmsg = self.debugformat % tuple(out)
+        return outmsg, pos
+
+class UnknownFormat:
+    name = '#unknown'
+    def parse(self, s, pos):
+        msgid = s[pos]
+        msg = str(bytearray(s))
+        return {'#msgid': msgid, '#msg': msg}, len(s)-MESSAGE_TRAILER_SIZE
+
+class MessageParser:
+    def __init__(self):
+        self.unknown = UnknownFormat()
+        self.messages_by_id = {}
+        self.messages_by_name = {}
+        self.static_strings = []
+        self.config = {}
+        self.version = ""
+        self.raw_identify_data = ""
+        self._init_messages(DefaultMessages, DefaultMessages.keys())
+    def check_packet(self, s):
+        if len(s) < MESSAGE_MIN:
+            return 0
+        msglen = ord(s[MESSAGE_POS_LEN])
+        if msglen < MESSAGE_MIN or msglen > MESSAGE_MAX:
+            return -1
+        msgseq = ord(s[MESSAGE_POS_SEQ])
+        if (msgseq & ~MESSAGE_SEQ_MASK) != MESSAGE_DEST:
+            return -1
+        if len(s) < msglen:
+            # Need more data
+            return 0
+        if s[msglen-MESSAGE_TRAILER_SYNC] != MESSAGE_SYNC:
+            return -1
+        msgcrc = s[msglen-MESSAGE_TRAILER_CRC:msglen-MESSAGE_TRAILER_CRC+2]
+        crc = crc16_ccitt(s[:msglen-MESSAGE_TRAILER_SIZE])
+        if crc != msgcrc:
+            #logging.debug("got crc %s vs %s" % (repr(crc), repr(msgcrc)))
+            return -1
+        return msglen
+    def dump(self, s):
+        msgseq = s[MESSAGE_POS_SEQ]
+        out = ["seq: %02x" % (msgseq,)]
+        pos = MESSAGE_HEADER_SIZE
+        while 1:
+            msgid = s[pos]
+            mid = self.messages_by_id.get(msgid, self.unknown)
+            params, pos = mid.dump(s, pos)
+            out.append("%s" % (params,))
+            if pos >= len(s)-MESSAGE_TRAILER_SIZE:
+                break
+        return out
+    def parse(self, s):
+        msgid = s[MESSAGE_HEADER_SIZE]
+        mid = self.messages_by_id.get(msgid, self.unknown)
+        params, pos = mid.parse(s, MESSAGE_HEADER_SIZE)
+        if pos != len(s)-MESSAGE_TRAILER_SIZE:
+            raise error("Extra data at end of message")
+        params['#name'] = mid.name
+        static_string_id = params.get('static_string_id')
+        if static_string_id is not None:
+            params['#msg'] = self.static_strings[static_string_id]
+        return params
+    def encode(self, seq, cmd):
+        msglen = MESSAGE_MIN + len(cmd)
+        seq = (seq & MESSAGE_SEQ_MASK) | MESSAGE_DEST
+        out = [chr(msglen), chr(seq), cmd]
+        out.append(crc16_ccitt(''.join(out)))
+        out.append(MESSAGE_SYNC)
+        return ''.join(out)
+    def _parse_buffer(self, value):
+        tval = int(value, 16)
+        out = []
+        for i in range(len(value)/2):
+            out.append(tval & 0xff)
+            tval >>= 8
+        out.reverse()
+        return ''.join([chr(i) for i in out])
+    def lookup_command(self, msgformat):
+        parts = msgformat.strip().split()
+        msgname = parts[0]
+        mp = self.messages_by_name.get(msgname)
+        if mp is None:
+            raise error("Unknown command: %s" % (msgname,))
+        if msgformat != mp.msgformat:
+            raise error("Command format mismatch: %s vs %s" % (
+                msgformat, mp.msgformat))
+        return mp
+    def create_command(self, msg):
+        parts = msg.strip().split()
+        if not parts:
+            return ""
+        msgname = parts[0]
+        mp = self.messages_by_name.get(msgname)
+        if mp is None:
+            raise error("Unknown command: %s" % (msgname,))
+        try:
+            argparts = dict(arg.split('=', 1) for arg in parts[1:])
+            for name, value in argparts.items():
+                t = mp.name_to_type[name]
+                if t.is_int:
+                    tval = int(value, 0)
+                else:
+                    tval = self._parse_buffer(value)
+                argparts[name] = tval
+        except:
+            #traceback.print_exc()
+            raise error("Unable to extract params from: %s" % (msgname,))
+        try:
+            cmd = mp.encode_by_name(**argparts)
+        except:
+            #traceback.print_exc()
+            raise error("Unable to encode: %s" % (msgname,))
+        return cmd
+    def _init_messages(self, messages, parsers):
+        for msgid, msgformat in messages.items():
+            msgid = int(msgid)
+            if msgid not in parsers:
+                self.messages_by_id[msgid] = OutputFormat(msgid, msgformat)
+                continue
+            msg = MessageFormat(msgid, msgformat)
+            self.messages_by_id[msgid] = msg
+            self.messages_by_name[msg.name] = msg
+    def process_identify(self, data, decompress=True):
+        if decompress:
+            data = zlib.decompress(data)
+        self.raw_identify_data = data
+        data = json.loads(data)
+        messages = data.get('messages')
+        commands = data.get('commands')
+        responses = data.get('responses')
+        self._init_messages(messages, commands+responses)
+        self.static_strings = data.get('static_strings', [])
+        self.config.update(data.get('config', {}))
+        self.version = data.get('version', '')
diff --git a/klippy/parsedump.py b/klippy/parsedump.py
new file mode 100755
index 00000000..0f6c48b3
--- /dev/null
+++ b/klippy/parsedump.py
@@ -0,0 +1,45 @@
+#!/usr/bin/env python
+# Script to parse a serial port data dump
+#
+# Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import os, sys, logging
+import msgproto
+
+def read_dictionary(filename):
+    dfile = open(filename, 'rb')
+    dictionary = dfile.read()
+    dfile.close()
+    return dictionary
+
+def main():
+    dict_filename, data_filename = sys.argv[1:]
+
+    dictionary = read_dictionary(dict_filename)
+
+    mp = msgproto.MessageParser()
+    mp.process_identify(dictionary, decompress=False)
+
+    f = open(data_filename, 'rb')
+    fd = f.fileno()
+    data = ""
+    while 1:
+        newdata = os.read(fd, 4096)
+        if not newdata:
+            break
+        data += newdata
+        while 1:
+            l = mp.check_packet(data)
+            if l == 0:
+                break
+            if l < 0:
+                logging.error("Invalid data")
+                data = data[-l:]
+                continue
+            msgs = mp.dump(bytearray(data[:l]))
+            sys.stdout.write('\n'.join(msgs[1:]) + '\n')
+            data = data[l:]
+
+if __name__ == '__main__':
+    main()
diff --git a/klippy/pins.py b/klippy/pins.py
new file mode 100644
index 00000000..fad41643
--- /dev/null
+++ b/klippy/pins.py
@@ -0,0 +1,88 @@
+# Pin name to pin number definitions
+#
+# Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+
+import re
+
+def avr_pins(port_count):
+    pins = {}
+    for port in range(port_count):
+        portchr = chr(65 + port)
+        if portchr == 'I':
+            continue
+        for portbit in range(8):
+            pins['P%c%d' % (portchr, portbit)] = port * 8 + portbit
+    return pins
+
+PINS_atmega164 = avr_pins(4)
+PINS_atmega1280 = avr_pins(12)
+
+MCU_PINS = {
+    "atmega168": PINS_atmega164, "atmega644p": PINS_atmega164,
+    "atmega1280": PINS_atmega1280, "atmega2560": PINS_atmega1280,
+}
+
+def mcu_to_pins(mcu):
+    return MCU_PINS.get(mcu, {})
+
+re_pin = re.compile(r'(?P<prefix>[ _]pin=)(?P<name>[^ ]*)')
+def update_command(cmd, pmap):
+    def fixup(m):
+        return m.group('prefix') + str(pmap[m.group('name')])
+    return re_pin.sub(fixup, cmd)
+
+
+######################################################################
+# Arduino mappings
+######################################################################
+
+Arduino_standard = [
+    "PD0", "PD1", "PD2", "PD3", "PD4", "PD5", "PD6", "PD7", "PB0", "PB1",
+    "PB2", "PB3", "PB4", "PB5", "PC0", "PC1", "PC2", "PC3", "PC4", "PC5",
+]
+Arduino_analog_standard = [
+    "PC0", "PC1", "PC2", "PC3", "PC4", "PC5", "PE0", "PE1",
+]
+
+Arduino_mega = [
+    "PE0", "PE1", "PE4", "PE5", "PG5", "PE3", "PH3", "PH4", "PH5", "PH6",
+    "PB4", "PB5", "PB6", "PB7", "PJ1", "PJ0", "PH1", "PH0", "PD3", "PD2",
+    "PD1", "PD0", "PA0", "PA1", "PA2", "PA3", "PA4", "PA5", "PA6", "PA7",
+    "PC7", "PC6", "PC5", "PC4", "PC3", "PC2", "PC1", "PC0", "PD7", "PG2",
+    "PG1", "PG0", "PL7", "PL6", "PL5", "PL4", "PL3", "PL2", "PL1", "PL0",
+    "PB3", "PB2", "PB1", "PB0", "PF0", "PF1", "PF2", "PF3", "PF4", "PF5",
+    "PF6", "PF7", "PK0", "PK1", "PK2", "PK3", "PK4", "PK5", "PK6", "PK7",
+]
+Arduino_analog_mega = [
+    "PF0", "PF1", "PF2", "PF3", "PF4", "PF5",
+    "PF6", "PF7", "PK0", "PK1", "PK2", "PK3", "PK4", "PK5", "PK6", "PK7",
+]
+
+Sanguino = [
+    "PB0", "PB1", "PB2", "PB3", "PB4", "PB5", "PB6", "PB7", "PD0", "PD1",
+    "PD2", "PD3", "PD4", "PD5", "PD6", "PD7", "PC0", "PC1", "PC2", "PC3",
+    "PC4", "PC5", "PC6", "PC7", "PA0", "PA1", "PA2", "PA3", "PA4", "PA5",
+    "PA6", "PA7"
+]
+Sanguino_analog = [
+    "PA0", "PA1", "PA2", "PA3", "PA4", "PA5", "PA6", "PA7"
+]
+
+Arduino_from_mcu = {
+    "atmega168": (Arduino_standard, Arduino_analog_standard),
+    "atmega644p": (Sanguino, Sanguino_analog),
+    "atmega1280": (Arduino_mega, Arduino_analog_mega),
+    "atmega2560": (Arduino_mega, Arduino_analog_mega),
+}
+
+def map_pins(name, mcu):
+    pins = MCU_PINS.get(mcu, {})
+    if name == 'arduino':
+        dpins, apins = Arduino_from_mcu.get(mcu, [])
+        for i in range(len(dpins)):
+            pins['ar' + str(i)] = pins[dpins[i]]
+        for i in range(len(apins)):
+            pins['analog%d' % (i,)] = pins[apins[i]]
+    return pins
diff --git a/klippy/reactor.py b/klippy/reactor.py
new file mode 100644
index 00000000..e07c6440
--- /dev/null
+++ b/klippy/reactor.py
@@ -0,0 +1,142 @@
+# File descriptor and timer event helper
+#
+# Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+
+import select, time, math
+
+class ReactorTimer:
+    def __init__(self, callback, waketime):
+        self.callback = callback
+        self.waketime = waketime
+
+class ReactorFileHandler:
+    def __init__(self, fd, callback):
+        self.fd = fd
+        self.callback = callback
+    def fileno(self):
+        return self.fd
+
+class SelectReactor:
+    NOW = 0.
+    NEVER = 9999999999999999.
+    def __init__(self):
+        self._fds = []
+        self._timers = []
+        self._next_timer = self.NEVER
+        self._process = True
+    # Timers
+    def _note_time(self, t):
+        nexttime = t.waketime
+        if nexttime < self._next_timer:
+            self._next_timer = nexttime
+    def update_timer(self, t, nexttime):
+        t.waketime = nexttime
+        self._note_time(t)
+    def register_timer(self, callback, waketime = NEVER):
+        handler = ReactorTimer(callback, waketime)
+        timers = list(self._timers)
+        timers.append(handler)
+        self._timers = timers
+        self._note_time(handler)
+        return handler
+    def unregister_timer(self, handler):
+        timers = list(self._timers)
+        timers.pop(timers.index(handler))
+        self._timers = timers
+    def _check_timers(self, eventtime):
+        if eventtime < self._next_timer:
+            return min(1., max(.001, self._next_timer - eventtime))
+        self._next_timer = self.NEVER
+        for t in self._timers:
+            if eventtime >= t.waketime:
+                t.waketime = t.callback(eventtime)
+            self._note_time(t)
+        if eventtime >= self._next_timer:
+            return 0.
+        return min(1., max(.001, self._next_timer - time.time()))
+    # File descriptors
+    def register_fd(self, fd, callback):
+        handler = ReactorFileHandler(fd, callback)
+        self._fds.append(handler)
+        return handler
+    def unregister_fd(self, handler):
+        self._fds.pop(self._fds.index(handler))
+    # Main loop
+    def run(self):
+        self._process = True
+        eventtime = time.time()
+        while self._process:
+            timeout = self._check_timers(eventtime)
+            res = select.select(self._fds, [], [], timeout)
+            eventtime = time.time()
+            for fd in res[0]:
+                fd.callback(eventtime)
+    def end(self):
+        self._process = False
+
+class PollReactor(SelectReactor):
+    def __init__(self):
+        SelectReactor.__init__(self)
+        self._poll = select.poll()
+        self._fds = {}
+    # File descriptors
+    def register_fd(self, fd, callback):
+        handler = ReactorFileHandler(fd, callback)
+        fds = self._fds.copy()
+        fds[fd] = callback
+        self._fds = fds
+        self._poll.register(handler, select.POLLIN | select.POLLHUP)
+        return handler
+    def unregister_fd(self, handler):
+        self._poll.unregister(handler)
+        fds = self._fds.copy()
+        del fds[handler.fd]
+        self._fds = fds
+    # Main loop
+    def run(self):
+        self._process = True
+        eventtime = time.time()
+        while self._process:
+            timeout = int(math.ceil(self._check_timers(eventtime) * 1000.))
+            res = self._poll.poll(timeout)
+            eventtime = time.time()
+            for fd, event in res:
+                self._fds[fd](eventtime)
+
+class EPollReactor(SelectReactor):
+    def __init__(self):
+        SelectReactor.__init__(self)
+        self._epoll = select.epoll()
+        self._fds = {}
+    # File descriptors
+    def register_fd(self, fd, callback):
+        handler = ReactorFileHandler(fd, callback)
+        fds = self._fds.copy()
+        fds[fd] = callback
+        self._fds = fds
+        self._epoll.register(fd, select.EPOLLIN | select.EPOLLHUP)
+        return handler
+    def unregister_fd(self, handler):
+        self._epoll.unregister(handler.fd)
+        fds = self._fds.copy()
+        del fds[handler.fd]
+        self._fds = fds
+    # Main loop
+    def run(self):
+        self._process = True
+        eventtime = time.time()
+        while self._process:
+            timeout = self._check_timers(eventtime)
+            res = self._epoll.poll(timeout)
+            eventtime = time.time()
+            for fd, event in res:
+                self._fds[fd](eventtime)
+
+# Use the poll based reactor if it is available
+try:
+    select.poll
+    Reactor = PollReactor
+except:
+    Reactor = SelectReactor
diff --git a/klippy/serialhdl.py b/klippy/serialhdl.py
new file mode 100644
index 00000000..80e29b96
--- /dev/null
+++ b/klippy/serialhdl.py
@@ -0,0 +1,286 @@
+# Serial port management for firmware communication
+#
+# Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import time, logging, threading
+import serial
+
+import msgproto, chelper
+
+class SerialReader:
+    BITS_PER_BYTE = 10
+    def __init__(self, reactor, serialport, baud):
+        self.reactor = reactor
+        self.serialport = serialport
+        self.baud = baud
+        # Serial port
+        self.ser = None
+        self.msgparser = msgproto.MessageParser()
+        # C interface
+        self.ffi_main, self.ffi_lib = chelper.get_ffi()
+        self.serialqueue = None
+        self.default_cmd_queue = self.alloc_command_queue()
+        self.stats_buf = self.ffi_main.new('char[4096]')
+        # MCU time/clock tracking
+        self.last_ack_time = self.last_ack_rtt_time = 0.
+        self.last_ack_clock = self.last_ack_rtt_clock = 0
+        self.est_clock = 0.
+        # Threading
+        self.lock = threading.Lock()
+        self.background_thread = None
+        # Message handlers
+        self.status_timer = self.reactor.register_timer(
+            self._status_event, self.reactor.NOW)
+        self.status_cmd = None
+        handlers = {
+            '#unknown': self.handle_unknown, '#state': self.handle_state,
+            '#output': self.handle_output, 'status': self.handle_status,
+            'shutdown': self.handle_output, 'is_shutdown': self.handle_output
+        }
+        self.handlers = dict(((k, None), v) for k, v in handlers.items())
+    def _bg_thread(self):
+        response = self.ffi_main.new('struct pull_queue_message *')
+        while 1:
+            self.ffi_lib.serialqueue_pull(self.serialqueue, response)
+            count = response.len
+            if count <= 0:
+                break
+            params = self.msgparser.parse(response.msg[0:count])
+            params['#sent_time'] = response.sent_time
+            params['#receive_time'] = response.receive_time
+            with self.lock:
+                hdl = (params['#name'], params.get('oid'))
+                hdl = self.handlers.get(hdl, self.handle_default)
+            try:
+                hdl(params)
+            except:
+                logging.exception("Exception in serial callback")
+    def connect(self):
+        logging.info("Starting serial connect")
+        self.ser = serial.Serial(self.serialport, self.baud, timeout=0)
+        stk500v2_leave(self.ser)
+        baud_adjust = float(self.BITS_PER_BYTE) / self.baud
+        self.serialqueue = self.ffi_lib.serialqueue_alloc(
+            self.ser.fileno(), baud_adjust, 0)
+        SerialBootStrap(self)
+        self.background_thread = threading.Thread(target=self._bg_thread)
+        self.background_thread.start()
+    def connect_file(self, debugoutput, dictionary, pace=False):
+        self.ser = debugoutput
+        self.msgparser.process_identify(dictionary, decompress=False)
+        baud_adjust = 0.
+        est_clock = 1000000000000.
+        if pace:
+            baud_adjust = float(self.BITS_PER_BYTE) / self.baud
+            est_clock = self.msgparser.config['CLOCK_FREQ']
+        self.serialqueue = self.ffi_lib.serialqueue_alloc(
+            self.ser.fileno(), baud_adjust, 1)
+        self.est_clock = est_clock
+        self.last_ack_time = time.time()
+        self.last_ack_clock = 0
+        self.ffi_lib.serialqueue_set_clock_est(
+            self.serialqueue, self.est_clock, self.last_ack_time
+            , self.last_ack_clock)
+    def disconnect(self):
+        self.send_flush()
+        time.sleep(0.010)
+        if self.ffi_lib is not None:
+            self.ffi_lib.serialqueue_exit(self.serialqueue)
+        if self.background_thread is not None:
+            self.background_thread.join()
+    def stats(self, eventtime):
+        if self.serialqueue is None:
+            return ""
+        sqstats = self.ffi_lib.serialqueue_get_stats(
+            self.serialqueue, self.stats_buf, len(self.stats_buf))
+        sqstats = self.ffi_main.string(self.stats_buf)
+        tstats = " est_clock=%.3f last_ack_time=%.3f last_ack_clock=%d" % (
+            self.est_clock, self.last_ack_time, self.last_ack_clock)
+        return sqstats + tstats
+    def _status_event(self, eventtime):
+        if self.status_cmd is None:
+            return eventtime + 0.1
+        self.send(self.status_cmd)
+        return eventtime + 1.0
+    # Serial response callbacks
+    def register_callback(self, callback, name, oid=None):
+        with self.lock:
+            self.handlers[name, oid] = callback
+    def unregister_callback(self, name, oid=None):
+        with self.lock:
+            del self.handlers[name, oid]
+    # Clock tracking
+    def get_clock(self, eventtime):
+        with self.lock:
+            return int(self.last_ack_clock
+                       + (eventtime - self.last_ack_time) * self.est_clock)
+    def translate_clock(self, raw_clock):
+        with self.lock:
+            last_ack_clock = self.last_ack_clock
+        clock_diff = (last_ack_clock - raw_clock) & 0xffffffff
+        if clock_diff & 0x80000000:
+            return last_ack_clock + 0x100000000 - clock_diff
+        return last_ack_clock - clock_diff
+    def get_last_clock(self):
+        with self.lock:
+            return self.last_ack_clock
+    # Command sending
+    def send(self, cmd, minclock=0, reqclock=0, cq=None):
+        if cq is None:
+            cq = self.default_cmd_queue
+        self.ffi_lib.serialqueue_send(
+            self.serialqueue, cq, cmd, len(cmd), minclock, reqclock)
+    def encode_and_send(self, data, minclock, reqclock, cq):
+        self.ffi_lib.serialqueue_encode_and_send(
+            self.serialqueue, cq, data, len(data), minclock, reqclock)
+    def send_with_response(self, cmd, callback, name):
+        SerialRetryCommand(self, cmd, callback, name)
+    def send_flush(self):
+        self.ffi_lib.serialqueue_flush_ready(self.serialqueue)
+    def alloc_command_queue(self):
+        return self.ffi_lib.serialqueue_alloc_commandqueue()
+    # Dumping debug lists
+    def dump_debug(self):
+        sdata = self.ffi_main.new('struct pull_queue_message[1024]')
+        rdata = self.ffi_main.new('struct pull_queue_message[1024]')
+        scount = self.ffi_lib.serialqueue_extract_old(
+            self.serialqueue, 1, sdata, len(sdata))
+        rcount = self.ffi_lib.serialqueue_extract_old(
+            self.serialqueue, 0, rdata, len(rdata))
+        logging.info("Dumping send queue %d messages" % (scount,))
+        for i in range(scount):
+            msg = sdata[i]
+            cmds = self.msgparser.dump(msg.msg[0:msg.len])
+            logging.info("Sent %d %f %f %d: %s" % (
+                i, msg.receive_time, msg.sent_time, msg.len, ', '.join(cmds)))
+        logging.info("Dumping receive queue %d messages" % (rcount,))
+        for i in range(rcount):
+            msg = rdata[i]
+            cmds = self.msgparser.dump(msg.msg[0:msg.len])
+            logging.info("Receive: %d %f %f %d: %s" % (
+                i, msg.receive_time, msg.sent_time, msg.len, ', '.join(cmds)))
+    # Default message handlers
+    def handle_status(self, params):
+        with self.lock:
+            # Update last_ack_time / last_ack_clock
+            ack_clock = (self.last_ack_clock & ~0xffffffff) | params['clock']
+            if ack_clock < self.last_ack_clock:
+                ack_clock += 0x100000000
+            sent_time = params['#sent_time']
+            self.last_ack_time = receive_time = params['#receive_time']
+            self.last_ack_clock = ack_clock
+            # Update est_clock (if applicable)
+            if receive_time > self.last_ack_rtt_time + 1. and sent_time:
+                if self.last_ack_rtt_time:
+                    timedelta = receive_time - self.last_ack_rtt_time
+                    clockdelta = ack_clock - self.last_ack_rtt_clock
+                    estclock = clockdelta / timedelta
+                    if estclock > self.est_clock and self.est_clock:
+                        self.est_clock = (self.est_clock * 63. + estclock) / 64.
+                    else:
+                        self.est_clock = estclock
+                self.last_ack_rtt_time = sent_time
+                self.last_ack_rtt_clock = ack_clock
+            self.ffi_lib.serialqueue_set_clock_est(
+                self.serialqueue, self.est_clock, receive_time, ack_clock)
+    def handle_unknown(self, params):
+        logging.warn("Unknown message type %d: %s" % (
+            params['#msgid'], repr(params['#msg'])))
+    def handle_output(self, params):
+        logging.info("%s: %s" % (params['#name'], params['#msg']))
+    def handle_state(self, params):
+        state = params['#state']
+        if state  == 'connected':
+            logging.info("Loaded %d commands (%s)" % (
+                len(self.msgparser.messages_by_id), self.msgparser.version))
+        else:
+            logging.info("State: %s" % (state,))
+    def handle_default(self, params):
+        logging.warn("got %s" % (params,))
+
+# Class to retry sending of a query command until a given response is received
+class SerialRetryCommand:
+    RETRY_TIME = 0.500
+    def __init__(self, serial, cmd, callback, name):
+        self.serial = serial
+        self.cmd = cmd
+        self.callback = callback
+        self.name = name
+        self.serial.register_callback(self.handle_callback, self.name)
+        self.send_timer = self.serial.reactor.register_timer(
+            self.send_event, self.serial.reactor.NOW)
+    def send_event(self, eventtime):
+        if self.callback is None:
+            self.serial.reactor.unregister_timer(self.send_timer)
+            return self.serial.reactor.NEVER
+        self.serial.send(self.cmd)
+        return eventtime + self.RETRY_TIME
+    def handle_callback(self, params):
+        done = self.callback(params)
+        if done:
+            self.serial.unregister_callback(self.name)
+            self.callback = None
+
+# Code to start communication and download message type dictionary
+class SerialBootStrap:
+    RETRY_TIME = 0.500
+    def __init__(self, serial):
+        self.serial = serial
+        self.identify_data = ""
+        self.identify_cmd = self.serial.msgparser.lookup_command(
+            "identify offset=%u count=%c")
+        self.is_done = False
+        self.serial.register_callback(self.handle_identify, 'identify_response')
+        self.serial.register_callback(self.handle_unknown, '#unknown')
+        self.send_timer = self.serial.reactor.register_timer(
+            self.send_event, self.serial.reactor.NOW)
+    def finalize(self):
+        self.is_done = True
+        self.serial.msgparser.process_identify(self.identify_data)
+        logging.info("MCU version: %s" % (self.serial.msgparser.version,))
+        self.serial.unregister_callback('identify_response')
+        self.serial.register_callback(self.serial.handle_unknown, '#unknown')
+        get_status = self.serial.msgparser.lookup_command('get_status')
+        self.serial.status_cmd = get_status.encode()
+        with self.serial.lock:
+            hdl = self.serial.handlers['#state', None]
+        statemsg = {'#name': '#state', '#state': 'connected'}
+        hdl(statemsg)
+    def handle_identify(self, params):
+        if self.is_done or params['offset'] != len(self.identify_data):
+            return
+        msgdata = params['data']
+        if not msgdata:
+            self.finalize()
+            return
+        self.identify_data += msgdata
+        imsg = self.identify_cmd.encode(len(self.identify_data), 40)
+        self.serial.send(imsg)
+    def send_event(self, eventtime):
+        if self.is_done:
+            self.serial.reactor.unregister_timer(self.send_timer)
+            return self.serial.reactor.NEVER
+        imsg = self.identify_cmd.encode(len(self.identify_data), 40)
+        self.serial.send(imsg)
+        return eventtime + self.RETRY_TIME
+    def handle_unknown(self, params):
+        logging.debug("Unknown message %d (len %d) while identifying" % (
+            params['#msgid'], len(params['#msg'])))
+
+# Attempt to place an AVR stk500v2 style programmer into normal mode
+def stk500v2_leave(ser):
+    logging.debug("Starting stk500v2 leave programmer sequence")
+    origbaud = ser.baudrate
+    # Request a dummy speed first as this seems to help reset the port
+    ser.baudrate = 1200
+    ser.read(1)
+    # Send stk500v2 leave programmer sequence
+    ser.baudrate = 115200
+    time.sleep(0.100)
+    ser.read(4096)
+    ser.write('\x1b\x01\x00\x01\x0e\x11\x04')
+    time.sleep(0.050)
+    res = ser.read(4096)
+    logging.debug("Got %s from stk500v2" % (repr(res),))
+    ser.baudrate = origbaud
diff --git a/klippy/serialqueue.c b/klippy/serialqueue.c
new file mode 100644
index 00000000..fa75701d
--- /dev/null
+++ b/klippy/serialqueue.c
@@ -0,0 +1,1021 @@
+// Serial port command queuing
+//
+// Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+//
+// This goal of this code is to handle low-level serial port
+// communications with a microcontroller (mcu).  This code is written
+// in C (instead of python) to reduce communication latencies and to
+// reduce scheduling jitter.  The code queues messages to be
+// transmitted, schedules transmission of commands at specified mcu
+// clock times, prioritizes commands, and handles retransmissions.  A
+// background thread is launched to do this work and minimize latency.
+
+#include <errno.h> // errno
+#include <math.h> // ceil
+#include <poll.h> // poll
+#include <pthread.h> // pthread_mutex_lock
+#include <stddef.h> // offsetof
+#include <stdint.h> // uint64_t
+#include <stdio.h> // snprintf
+#include <stdlib.h> // malloc
+#include <string.h> // memset
+#include <sys/time.h> // gettimeofday
+#include <time.h> // struct timespec
+#include <termios.h> // tcflush
+#include <unistd.h> // pipe
+#include "list.h" // list_add_tail
+#include "serialqueue.h" // struct queue_message
+
+
+/****************************************************************
+ * Helper functions
+ ****************************************************************/
+
+// Return the current system time as a double
+static double
+get_time(void)
+{
+    struct timeval tv;
+    gettimeofday(&tv, NULL);
+    return (double)tv.tv_sec + (double)tv.tv_usec / 1000000.;
+}
+
+#if 0
+// Fill a 'struct timespec' with a system time stored in a double
+struct timespec
+fill_time(double time)
+{
+    time_t t = time;
+    return (struct timespec) {t, (time - t)*1000000000. };
+}
+#endif
+
+// Report 'errno' in a message written to stderr
+void
+report_errno(char *where, int rc)
+{
+    int e = errno;
+    fprintf(stderr, "Got error %d in %s: (%d)%s\n", rc, where, e, strerror(e));
+}
+
+
+/****************************************************************
+ * Poll reactor
+ ****************************************************************/
+
+// The 'poll reactor' code is a mechanism for dispatching timer and
+// file descriptor events.
+
+#define PR_NOW   0.
+#define PR_NEVER 9999999999999999.
+
+struct pollreactor_timer {
+    double waketime;
+    double (*callback)(void *data, double eventtime);
+};
+
+struct pollreactor {
+    int num_fds, num_timers, must_exit;
+    void *callback_data;
+    double next_timer;
+    struct pollfd *fds;
+    void (**fd_callbacks)(void *data, double eventtime);
+    struct pollreactor_timer *timers;
+};
+
+// Allocate a new 'struct pollreactor' object
+static void
+pollreactor_setup(struct pollreactor *pr, int num_fds, int num_timers
+                  , void *callback_data)
+{
+    pr->num_fds = num_fds;
+    pr->num_timers = num_timers;
+    pr->must_exit = 0;
+    pr->callback_data = callback_data;
+    pr->next_timer = PR_NEVER;
+    pr->fds = malloc(num_fds * sizeof(*pr->fds));
+    memset(pr->fds, 0, num_fds * sizeof(*pr->fds));
+    pr->fd_callbacks = malloc(num_fds * sizeof(*pr->fd_callbacks));
+    memset(pr->fd_callbacks, 0, num_fds * sizeof(*pr->fd_callbacks));
+    pr->timers = malloc(num_timers * sizeof(*pr->timers));
+    memset(pr->timers, 0, num_timers * sizeof(*pr->timers));
+    int i;
+    for (i=0; i<num_timers; i++)
+        pr->timers[i].waketime = PR_NEVER;
+}
+
+// Add a callback for when a file descriptor (fd) becomes readable
+static void
+pollreactor_add_fd(struct pollreactor *pr, int pos, int fd, void *callback)
+{
+    pr->fds[pos].fd = fd;
+    pr->fds[pos].events = POLLIN|POLLHUP;
+    pr->fds[pos].revents = 0;
+    pr->fd_callbacks[pos] = callback;
+}
+
+// Add a timer callback
+static void
+pollreactor_add_timer(struct pollreactor *pr, int pos, void *callback)
+{
+    pr->timers[pos].callback = callback;
+    pr->timers[pos].waketime = PR_NEVER;
+}
+
+#if 0
+// Return the last schedule wake-up time for a timer
+static double
+pollreactor_get_timer(struct pollreactor *pr, int pos)
+{
+    return pr->timers[pos].waketime;
+}
+#endif
+
+// Set the wake-up time for a given timer
+static void
+pollreactor_update_timer(struct pollreactor *pr, int pos, double waketime)
+{
+    pr->timers[pos].waketime = waketime;
+    if (waketime < pr->next_timer)
+        pr->next_timer = waketime;
+}
+
+// Internal code to invoke timer callbacks
+static int
+pollreactor_check_timers(struct pollreactor *pr, double eventtime)
+{
+    if (eventtime >= pr->next_timer) {
+        pr->next_timer = PR_NEVER;
+        int i;
+        for (i=0; i<pr->num_timers; i++) {
+            struct pollreactor_timer *timer = &pr->timers[i];
+            double t = timer->waketime;
+            if (eventtime >= t) {
+                t = timer->callback(pr->callback_data, eventtime);
+                timer->waketime = t;
+            }
+            if (t < pr->next_timer)
+                pr->next_timer = t;
+        }
+        if (eventtime >= pr->next_timer)
+            return 0;
+    }
+    double timeout = ceil((pr->next_timer - eventtime) * 1000.);
+    return timeout < 1. ? 1 : (timeout > 1000. ? 1000 : (int)timeout);
+}
+
+// Repeatedly check for timer and fd events and invoke their callbacks
+static void
+pollreactor_run(struct pollreactor *pr)
+{
+    pr->must_exit = 0;
+    double eventtime = get_time();
+    while (! pr->must_exit) {
+        int timeout = pollreactor_check_timers(pr, eventtime);
+        int ret = poll(pr->fds, pr->num_fds, timeout);
+        eventtime = get_time();
+        if (ret > 0) {
+            int i;
+            for (i=0; i<pr->num_fds; i++)
+                if (pr->fds[i].revents)
+                    pr->fd_callbacks[i](pr->callback_data, eventtime);
+        } else if (ret < 0) {
+            report_errno("poll", ret);
+            pr->must_exit = 1;
+        }
+    }
+}
+
+// Request that a currently running pollreactor_run() loop exit
+static void
+pollreactor_do_exit(struct pollreactor *pr)
+{
+    pr->must_exit = 1;
+}
+
+// Check if a pollreactor_run() loop has been requested to exit
+static int
+pollreactor_is_exit(struct pollreactor *pr)
+{
+    return pr->must_exit;
+}
+
+
+/****************************************************************
+ * Serial protocol helpers
+ ****************************************************************/
+
+// Implement the standard crc "ccitt" algorithm on the given buffer
+static uint16_t
+crc16_ccitt(uint8_t *buf, uint8_t len)
+{
+    uint16_t crc = 0xffff;
+    while (len--) {
+        uint8_t data = *buf++;
+        data ^= crc & 0xff;
+        data ^= data << 4;
+        crc = ((((uint16_t)data << 8) | (crc >> 8)) ^ (uint8_t)(data >> 4)
+               ^ ((uint16_t)data << 3));
+    }
+    return crc;
+}
+
+// Verify a buffer starts with a valid mcu message
+static int
+check_message(uint8_t *need_sync, uint8_t *buf, int buf_len)
+{
+    if (buf_len < MESSAGE_MIN)
+        // Need more data
+        return 0;
+    if (*need_sync)
+        goto error;
+    uint8_t msglen = buf[MESSAGE_POS_LEN];
+    if (msglen < MESSAGE_MIN || msglen > MESSAGE_MAX)
+        goto error;
+    uint8_t msgseq = buf[MESSAGE_POS_SEQ];
+    if ((msgseq & ~MESSAGE_SEQ_MASK) != MESSAGE_DEST)
+        goto error;
+    if (buf_len < msglen)
+        // Need more data
+        return 0;
+    if (buf[msglen-MESSAGE_TRAILER_SYNC] != MESSAGE_SYNC)
+        goto error;
+    uint16_t msgcrc = ((buf[msglen-MESSAGE_TRAILER_CRC] << 8)
+                       | (uint8_t)buf[msglen-MESSAGE_TRAILER_CRC+1]);
+    uint16_t crc = crc16_ccitt(buf, msglen-MESSAGE_TRAILER_SIZE);
+    if (crc != msgcrc)
+        goto error;
+    return msglen;
+
+error: ;
+    // Discard bytes until next SYNC found
+    uint8_t *next_sync = memchr(buf, MESSAGE_SYNC, buf_len);
+    if (next_sync) {
+        *need_sync = 0;
+        return -(next_sync - buf + 1);
+    }
+    *need_sync = 1;
+    return -buf_len;
+}
+
+// Encode an integer as a variable length quantity (vlq)
+static uint8_t *
+encode_int(uint8_t *p, uint32_t v)
+{
+    int32_t sv = v;
+    if (sv < (3L<<5)  && sv >= -(1L<<5))  goto f4;
+    if (sv < (3L<<12) && sv >= -(1L<<12)) goto f3;
+    if (sv < (3L<<19) && sv >= -(1L<<19)) goto f2;
+    if (sv < (3L<<26) && sv >= -(1L<<26)) goto f1;
+    *p++ = (v>>28) | 0x80;
+f1: *p++ = ((v>>21) & 0x7f) | 0x80;
+f2: *p++ = ((v>>14) & 0x7f) | 0x80;
+f3: *p++ = ((v>>7) & 0x7f) | 0x80;
+f4: *p++ = v & 0x7f;
+    return p;
+}
+
+
+/****************************************************************
+ * Command queues
+ ****************************************************************/
+
+struct command_queue {
+    struct list_head stalled_queue, ready_queue;
+    struct list_node node;
+};
+
+// Allocate a 'struct queue_message' object
+static struct queue_message *
+message_alloc(void)
+{
+    struct queue_message *qm = malloc(sizeof(*qm));
+    memset(qm, 0, sizeof(*qm));
+    return qm;
+}
+
+// Allocate a queue_message and fill it with the specified data
+static struct queue_message *
+message_fill(uint8_t *data, int len)
+{
+    struct queue_message *qm = message_alloc();
+    memcpy(qm->msg, data, len);
+    qm->len = len;
+    return qm;
+}
+
+// Allocate a queue_message and fill it with a series of encoded vlq integers
+struct queue_message *
+message_alloc_and_encode(uint32_t *data, int len)
+{
+    struct queue_message *qm = message_alloc();
+    int i;
+    uint8_t *p = qm->msg;
+    for (i=0; i<len; i++) {
+        p = encode_int(p, data[i]);
+        if (p > &qm->msg[MESSAGE_PAYLOAD_MAX])
+            goto fail;
+    }
+    qm->len = p - qm->msg;
+    return qm;
+
+fail:
+    fprintf(stderr, "Encode error\n");
+    qm->len = 0;
+    return qm;
+}
+
+// Free the storage from a previous message_alloc() call
+static void
+message_free(struct queue_message *qm)
+{
+    free(qm);
+}
+
+
+/****************************************************************
+ * Serialqueue interface
+ ****************************************************************/
+
+struct serialqueue {
+    // Input reading
+    struct pollreactor pr;
+    int serial_fd;
+    int pipe_fds[2];
+    uint8_t input_buf[4096];
+    uint8_t need_sync;
+    int input_pos;
+    // Threading
+    pthread_t tid;
+    pthread_mutex_t lock; // protects variables below
+    pthread_cond_t cond;
+    int receive_waiting;
+    // Baud / clock tracking
+    double baud_adjust, idle_time;
+    double est_clock, last_ack_time;
+    uint64_t last_ack_clock;
+    double last_receive_sent_time;
+    // Retransmit support
+    uint64_t send_seq, receive_seq;
+    uint64_t retransmit_seq, rtt_sample_seq;
+    struct list_head sent_queue;
+    double srtt, rttvar, rto;
+    // Pending transmission message queues
+    struct list_head pending_queues;
+    int ready_bytes, stalled_bytes;
+    uint64_t need_kick_clock;
+    int can_delay_writes;
+    // Received messages
+    struct list_head receive_queue;
+    // Debugging
+    struct list_head old_sent, old_receive;
+    // Stats
+    uint32_t bytes_write, bytes_read, bytes_retransmit, bytes_invalid;
+};
+
+#define SQPF_SERIAL 0
+#define SQPF_PIPE   1
+#define SQPF_NUM    2
+
+#define SQPT_RETRANSMIT 0
+#define SQPT_COMMAND    1
+#define SQPT_NUM        2
+
+#define MIN_RTO 0.025
+#define MAX_RTO 5.000
+#define MAX_SERIAL_BUFFER 0.050
+#define MIN_REQTIME_DELTA 0.250
+#define IDLE_QUERY_TIME 1.0
+
+#define DEBUG_QUEUE_SENT 100
+#define DEBUG_QUEUE_RECEIVE 20
+
+// Create a series of empty messages and add them to a list
+static void
+debug_queue_alloc(struct list_head *root, int count)
+{
+    int i;
+    for (i=0; i<count; i++) {
+        struct queue_message *qm = message_alloc();
+        list_add_head(&qm->node, root);
+    }
+}
+
+// Copy a message to a debug queue and free old debug messages
+static void
+debug_queue_add(struct list_head *root, struct queue_message *qm)
+{
+    list_add_tail(&qm->node, root);
+    struct queue_message *old = list_first_entry(
+        root, struct queue_message, node);
+    list_del(&old->node);
+    message_free(old);
+}
+
+// Wake up the receiver thread if it is waiting
+static void
+check_wake_receive(struct serialqueue *sq)
+{
+    if (sq->receive_waiting) {
+        sq->receive_waiting = 0;
+        pthread_cond_signal(&sq->cond);
+    }
+}
+
+// Update internal state when the receive sequence increases
+static void
+update_receive_seq(struct serialqueue *sq, double eventtime, uint64_t rseq)
+{
+    // Remove from sent queue
+    int ack_count = rseq - sq->receive_seq;
+    uint64_t sent_seq = sq->receive_seq;
+    while (!list_empty(&sq->sent_queue) && ack_count--) {
+        struct queue_message *sent = list_first_entry(
+            &sq->sent_queue, struct queue_message, node);
+        if (rseq == ++sent_seq)
+            sq->last_receive_sent_time = sent->receive_time;
+        list_del(&sent->node);
+        debug_queue_add(&sq->old_sent, sent);
+    }
+    sq->receive_seq = rseq;
+    if (rseq > sq->send_seq)
+        sq->send_seq = rseq;
+    pollreactor_update_timer(&sq->pr, SQPT_COMMAND, PR_NOW);
+
+    // Update retransmit info
+    if (sq->rtt_sample_seq && rseq >= sq->rtt_sample_seq
+        && sq->last_receive_sent_time) {
+        // RFC6298 rtt calculations
+        double delta = eventtime - sq->last_receive_sent_time;
+        if (!sq->srtt) {
+            sq->rttvar = delta / 2.0;
+            sq->srtt = delta * 10.0; // use a higher start default
+        } else {
+            sq->rttvar = (3.0 * sq->rttvar + fabs(sq->srtt - delta)) / 4.0;
+            sq->srtt = (7.0 * sq->srtt + delta) / 8.0;
+        }
+        double rttvar4 = sq->rttvar * 4.0;
+        if (rttvar4 < 0.001)
+            rttvar4 = 0.001;
+        sq->rto = sq->srtt + rttvar4;
+        if (sq->rto < MIN_RTO)
+            sq->rto = MIN_RTO;
+        else if (sq->rto > MAX_RTO)
+            sq->rto = MAX_RTO;
+        sq->rtt_sample_seq = 0;
+    }
+    if (list_empty(&sq->sent_queue)) {
+        pollreactor_update_timer(&sq->pr, SQPT_RETRANSMIT, PR_NEVER);
+    } else {
+        struct queue_message *sent = list_first_entry(
+            &sq->sent_queue, struct queue_message, node);
+        double nr = eventtime + sq->rto + sent->len * sq->baud_adjust;
+        pollreactor_update_timer(&sq->pr, SQPT_RETRANSMIT, nr);
+    }
+}
+
+// Process a well formed input message
+static void
+handle_message(struct serialqueue *sq, double eventtime, int len)
+{
+    // Calculate receive sequence number
+    uint64_t rseq = ((sq->receive_seq & ~MESSAGE_SEQ_MASK)
+                     | (sq->input_buf[MESSAGE_POS_SEQ] & MESSAGE_SEQ_MASK));
+    if (rseq < sq->receive_seq)
+        rseq += MESSAGE_SEQ_MASK+1;
+
+    if (rseq != sq->receive_seq)
+        // New sequence number
+        update_receive_seq(sq, eventtime, rseq);
+    else if (len == MESSAGE_MIN && rseq > sq->retransmit_seq)
+        // Duplicate sequence number in an empty message is a nak
+        pollreactor_update_timer(&sq->pr, SQPT_RETRANSMIT, PR_NOW);
+
+    if (len > MESSAGE_MIN) {
+        // Add message to receive queue
+        struct queue_message *qm = message_fill(sq->input_buf, len);
+        qm->sent_time = sq->last_receive_sent_time;
+        qm->receive_time = eventtime;
+        list_add_tail(&qm->node, &sq->receive_queue);
+        check_wake_receive(sq);
+    }
+}
+
+// Callback for input activity on the serial fd
+static void
+input_event(struct serialqueue *sq, double eventtime)
+{
+    int ret = read(sq->serial_fd, &sq->input_buf[sq->input_pos]
+                   , sizeof(sq->input_buf) - sq->input_pos);
+    if (ret <= 0) {
+        report_errno("read", ret);
+        pollreactor_do_exit(&sq->pr);
+        return;
+    }
+    sq->input_pos += ret;
+    for (;;) {
+        ret = check_message(&sq->need_sync, sq->input_buf, sq->input_pos);
+        if (!ret)
+            // Need more data
+            return;
+        if (ret > 0) {
+            // Received a valid message
+            pthread_mutex_lock(&sq->lock);
+            handle_message(sq, eventtime, ret);
+            sq->bytes_read += ret;
+            pthread_mutex_unlock(&sq->lock);
+        } else {
+            // Skip bad data at beginning of input
+            ret = -ret;
+            pthread_mutex_lock(&sq->lock);
+            sq->bytes_invalid += ret;
+            pthread_mutex_unlock(&sq->lock);
+        }
+        sq->input_pos -= ret;
+        if (sq->input_pos)
+            memmove(sq->input_buf, &sq->input_buf[ret], sq->input_pos);
+    }
+}
+
+// Callback for input activity on the pipe fd (wakes command_event)
+static void
+kick_event(struct serialqueue *sq, double eventtime)
+{
+    char dummy[4096];
+    int ret = read(sq->pipe_fds[0], dummy, sizeof(dummy));
+    if (ret < 0)
+        report_errno("pipe read", ret);
+    pollreactor_update_timer(&sq->pr, SQPT_COMMAND, PR_NOW);
+}
+
+// Callback timer for when a retransmit should be done
+static double
+retransmit_event(struct serialqueue *sq, double eventtime)
+{
+    int ret = tcflush(sq->serial_fd, TCOFLUSH);
+    if (ret < 0)
+        report_errno("tcflush", ret);
+
+    pthread_mutex_lock(&sq->lock);
+
+    // Retransmit all pending messages
+    uint8_t buf[MESSAGE_MAX * MESSAGE_SEQ_MASK + 1];
+    int buflen = 0;
+    buf[buflen++] = MESSAGE_SYNC;
+    struct queue_message *qm;
+    list_for_each_entry(qm, &sq->sent_queue, node) {
+        memcpy(&buf[buflen], qm->msg, qm->len);
+        buflen += qm->len;
+    }
+    ret = write(sq->serial_fd, buf, buflen);
+    if (ret < 0)
+        report_errno("retransmit write", ret);
+    sq->bytes_retransmit += buflen;
+
+    // Update rto
+    sq->rto *= 2.0;
+    if (sq->rto > MAX_RTO)
+        sq->rto = MAX_RTO;
+    sq->retransmit_seq = sq->send_seq;
+    sq->rtt_sample_seq = 0;
+    sq->idle_time = eventtime + buflen * sq->baud_adjust;
+    double waketime = sq->idle_time + sq->rto;
+
+    pthread_mutex_unlock(&sq->lock);
+    return waketime;
+}
+
+// Construct a block of data and send to the serial port
+static void
+build_and_send_command(struct serialqueue *sq, double eventtime)
+{
+    struct queue_message *out = message_alloc();
+    out->len = MESSAGE_HEADER_SIZE;
+
+    while (sq->ready_bytes) {
+        // Find highest priority message (message with lowest req_clock)
+        uint64_t min_clock = MAX_CLOCK;
+        struct command_queue *q, *cq = NULL;
+        struct queue_message *qm = NULL;
+        list_for_each_entry(q, &sq->pending_queues, node) {
+            if (!list_empty(&q->ready_queue)) {
+                struct queue_message *m = list_first_entry(
+                    &q->ready_queue, struct queue_message, node);
+                if (m->req_clock < min_clock) {
+                    min_clock = m->req_clock;
+                    cq = q;
+                    qm = m;
+                }
+            }
+        }
+        // Append message to outgoing command
+        if (out->len + qm->len > sizeof(out->msg) - MESSAGE_TRAILER_SIZE)
+            break;
+        list_del(&qm->node);
+        if (list_empty(&cq->ready_queue) && list_empty(&cq->stalled_queue))
+            list_del(&cq->node);
+        memcpy(&out->msg[out->len], qm->msg, qm->len);
+        out->len += qm->len;
+        sq->ready_bytes -= qm->len;
+        message_free(qm);
+    }
+
+    // Fill header / trailer
+    out->len += MESSAGE_TRAILER_SIZE;
+    out->msg[MESSAGE_POS_LEN] = out->len;
+    out->msg[MESSAGE_POS_SEQ] = MESSAGE_DEST | (sq->send_seq & MESSAGE_SEQ_MASK);
+    uint16_t crc = crc16_ccitt(out->msg, out->len - MESSAGE_TRAILER_SIZE);
+    out->msg[out->len - MESSAGE_TRAILER_CRC] = crc >> 8;
+    out->msg[out->len - MESSAGE_TRAILER_CRC+1] = crc & 0xff;
+    out->msg[out->len - MESSAGE_TRAILER_SYNC] = MESSAGE_SYNC;
+
+    // Send message
+    int ret = write(sq->serial_fd, out->msg, out->len);
+    if (ret < 0)
+        report_errno("write", ret);
+    sq->bytes_write += out->len;
+    if (eventtime > sq->idle_time)
+        sq->idle_time = eventtime;
+    sq->idle_time += out->len * sq->baud_adjust;
+    out->sent_time = eventtime;
+    out->receive_time = sq->idle_time;
+    if (list_empty(&sq->sent_queue))
+        pollreactor_update_timer(&sq->pr, SQPT_RETRANSMIT
+                                 , sq->idle_time + sq->rto);
+    sq->send_seq++;
+    if (!sq->rtt_sample_seq)
+        sq->rtt_sample_seq = sq->send_seq;
+    list_add_tail(&out->node, &sq->sent_queue);
+}
+
+// Determine the time the next serial data should be sent
+static double
+check_send_command(struct serialqueue *sq, double eventtime)
+{
+    if (eventtime < sq->idle_time - MAX_SERIAL_BUFFER)
+        // Serial port already busy
+        return sq->idle_time - MAX_SERIAL_BUFFER;
+    if (sq->send_seq - sq->receive_seq >= MESSAGE_SEQ_MASK
+        && sq->receive_seq != (uint64_t)-1)
+        // Need an ack before more messages can be sent
+        return PR_NEVER;
+
+    // Check for stalled messages now ready
+    double idletime = eventtime > sq->idle_time ? eventtime : sq->idle_time;
+    idletime += MESSAGE_MIN * sq->baud_adjust;
+    double timedelta = idletime - sq->last_ack_time;
+    uint64_t ack_clock = (uint64_t)(timedelta * sq->est_clock) + sq->last_ack_clock;
+    uint64_t min_stalled_clock = MAX_CLOCK, min_ready_clock = MAX_CLOCK;
+    struct command_queue *cq;
+    list_for_each_entry(cq, &sq->pending_queues, node) {
+        // Move messages from the stalled_queue to the ready_queue
+        while (!list_empty(&cq->stalled_queue)) {
+            struct queue_message *qm = list_first_entry(
+                &cq->stalled_queue, struct queue_message, node);
+            if (ack_clock < qm->min_clock) {
+                if (qm->min_clock < min_stalled_clock)
+                    min_stalled_clock = qm->min_clock;
+                break;
+            }
+            list_del(&qm->node);
+            list_add_tail(&qm->node, &cq->ready_queue);
+            sq->stalled_bytes -= qm->len;
+            sq->ready_bytes += qm->len;
+        }
+        // Update min_ready_clock
+        if (!list_empty(&cq->ready_queue)) {
+            struct queue_message *qm = list_first_entry(
+                &cq->ready_queue, struct queue_message, node);
+            if (qm->req_clock < min_ready_clock)
+                min_ready_clock = qm->req_clock;
+        }
+    }
+
+    // Check for messages to send
+    if (sq->ready_bytes >= MESSAGE_PAYLOAD_MAX)
+        return PR_NOW;
+    if (! sq->can_delay_writes) {
+        if (sq->ready_bytes)
+            return PR_NOW;
+        if (sq->est_clock)
+            sq->can_delay_writes = 1;
+        sq->need_kick_clock = MAX_CLOCK;
+        return PR_NEVER;
+    }
+    uint64_t reqclock_delta = MIN_REQTIME_DELTA * sq->est_clock;
+    if (min_ready_clock <= ack_clock + reqclock_delta)
+        return PR_NOW;
+    uint64_t wantclock = min_ready_clock - reqclock_delta;
+    if (min_stalled_clock < wantclock)
+        wantclock = min_stalled_clock;
+    sq->need_kick_clock = wantclock;
+    return idletime + (wantclock - ack_clock) / sq->est_clock;
+}
+
+// Callback timer to send data to the serial port
+static double
+command_event(struct serialqueue *sq, double eventtime)
+{
+    pthread_mutex_lock(&sq->lock);
+    double waketime;
+    for (;;) {
+        waketime = check_send_command(sq, eventtime);
+        if (waketime != PR_NOW)
+            break;
+        build_and_send_command(sq, eventtime);
+    }
+    pthread_mutex_unlock(&sq->lock);
+    return waketime;
+}
+
+// Main background thread for reading/writing to serial port
+static void *
+background_thread(void *data)
+{
+    struct serialqueue *sq = data;
+    pollreactor_run(&sq->pr);
+
+    pthread_mutex_lock(&sq->lock);
+    check_wake_receive(sq);
+    pthread_mutex_unlock(&sq->lock);
+
+    return NULL;
+}
+
+// Create a new 'struct serialqueue' object
+struct serialqueue *
+serialqueue_alloc(int serial_fd, double baud_adjust, int write_only)
+{
+    struct serialqueue *sq = malloc(sizeof(*sq));
+    memset(sq, 0, sizeof(*sq));
+    sq->baud_adjust = baud_adjust;
+
+    // Reactor setup
+    sq->serial_fd = serial_fd;
+    int ret = pipe(sq->pipe_fds);
+    if (ret)
+        goto fail;
+    pollreactor_setup(&sq->pr, SQPF_NUM, SQPT_NUM, sq);
+    if (!write_only)
+        pollreactor_add_fd(&sq->pr, SQPF_SERIAL, serial_fd, input_event);
+    pollreactor_add_fd(&sq->pr, SQPF_PIPE, sq->pipe_fds[0], kick_event);
+    pollreactor_add_timer(&sq->pr, SQPT_RETRANSMIT, retransmit_event);
+    pollreactor_add_timer(&sq->pr, SQPT_COMMAND, command_event);
+
+    // Retransmit setup
+    sq->send_seq = 1;
+    if (write_only) {
+        sq->receive_seq = -1;
+        sq->rto = PR_NEVER;
+    } else {
+        sq->receive_seq = 1;
+        sq->rto = MIN_RTO;
+    }
+
+    // Queues
+    sq->need_kick_clock = MAX_CLOCK;
+    list_init(&sq->pending_queues);
+    list_init(&sq->sent_queue);
+    list_init(&sq->receive_queue);
+
+    // Debugging
+    list_init(&sq->old_sent);
+    list_init(&sq->old_receive);
+    debug_queue_alloc(&sq->old_sent, DEBUG_QUEUE_SENT);
+    debug_queue_alloc(&sq->old_receive, DEBUG_QUEUE_RECEIVE);
+
+    // Thread setup
+    ret = pthread_mutex_init(&sq->lock, NULL);
+    if (ret)
+        goto fail;
+    ret = pthread_cond_init(&sq->cond, NULL);
+    if (ret)
+        goto fail;
+    ret = pthread_create(&sq->tid, NULL, background_thread, sq);
+    if (ret)
+        goto fail;
+
+    return sq;
+
+fail:
+    report_errno("init", ret);
+    return NULL;
+}
+
+// Request that the background thread exit
+void
+serialqueue_exit(struct serialqueue *sq)
+{
+    pollreactor_do_exit(&sq->pr);
+    int ret = pthread_join(sq->tid, NULL);
+    if (ret)
+        report_errno("pthread_join", ret);
+}
+
+// Allocate a 'struct command_queue'
+struct command_queue *
+serialqueue_alloc_commandqueue(void)
+{
+    struct command_queue *cq = malloc(sizeof(*cq));
+    memset(cq, 0, sizeof(*cq));
+    list_init(&cq->ready_queue);
+    list_init(&cq->stalled_queue);
+    return cq;
+}
+
+// Write to the internal pipe to wake the background thread if in poll
+static void
+kick_bg_thread(struct serialqueue *sq)
+{
+    int ret = write(sq->pipe_fds[1], ".", 1);
+    if (ret < 0)
+        report_errno("pipe write", ret);
+}
+
+// Add a batch of messages to the given command_queue
+void
+serialqueue_send_batch(struct serialqueue *sq, struct command_queue *cq
+                       , struct list_head *msgs)
+{
+    // Make sure min_clock is set in list and calculate total bytes
+    int len = 0;
+    struct queue_message *qm;
+    list_for_each_entry(qm, msgs, node) {
+        if (qm->min_clock + (1LL<<31) < qm->req_clock)
+            qm->min_clock = qm->req_clock - (1LL<<31);
+        len += qm->len;
+    }
+    if (! len)
+        return;
+    qm = list_first_entry(msgs, struct queue_message, node);
+
+    // Add list to cq->stalled_queue
+    pthread_mutex_lock(&sq->lock);
+    if (list_empty(&cq->ready_queue) && list_empty(&cq->stalled_queue))
+        list_add_tail(&cq->node, &sq->pending_queues);
+    list_join_tail(msgs, &cq->stalled_queue);
+    sq->stalled_bytes += len;
+    int mustwake = 0;
+    if (qm->min_clock < sq->need_kick_clock) {
+        sq->need_kick_clock = 0;
+        mustwake = 1;
+    }
+    pthread_mutex_unlock(&sq->lock);
+
+    // Wake the background thread if necessary
+    if (mustwake)
+        kick_bg_thread(sq);
+}
+
+// Schedule the transmission of a message on the serial port at a
+// given time and priority.
+void
+serialqueue_send(struct serialqueue *sq, struct command_queue *cq
+                 , uint8_t *msg, int len, uint64_t min_clock, uint64_t req_clock)
+{
+    struct queue_message *qm = message_fill(msg, len);
+    qm->min_clock = min_clock;
+    qm->req_clock = req_clock;
+
+    struct list_head msgs;
+    list_init(&msgs);
+    list_add_tail(&qm->node, &msgs);
+    serialqueue_send_batch(sq, cq, &msgs);
+}
+
+// Like serialqueue_send() but also builds the message to be sent
+void
+serialqueue_encode_and_send(struct serialqueue *sq, struct command_queue *cq
+                            , uint32_t *data, int len
+                            , uint64_t min_clock, uint64_t req_clock)
+{
+    struct queue_message *qm = message_alloc_and_encode(data, len);
+    qm->min_clock = min_clock;
+    qm->req_clock = req_clock;
+
+    struct list_head msgs;
+    list_init(&msgs);
+    list_add_tail(&qm->node, &msgs);
+    serialqueue_send_batch(sq, cq, &msgs);
+}
+
+// Return a message read from the serial port (or wait for one if none
+// available)
+void
+serialqueue_pull(struct serialqueue *sq, struct pull_queue_message *pqm)
+{
+    pthread_mutex_lock(&sq->lock);
+    // Wait for message to be available
+    while (list_empty(&sq->receive_queue)) {
+        if (pollreactor_is_exit(&sq->pr))
+            goto exit;
+        sq->receive_waiting = 1;
+        int ret = pthread_cond_wait(&sq->cond, &sq->lock);
+        if (ret)
+            report_errno("pthread_cond_wait", ret);
+    }
+
+    // Remove message from queue
+    struct queue_message *qm = list_first_entry(
+        &sq->receive_queue, struct queue_message, node);
+    list_del(&qm->node);
+
+    // Copy message
+    memcpy(pqm->msg, qm->msg, qm->len);
+    pqm->len = qm->len;
+    pqm->sent_time = qm->sent_time;
+    pqm->receive_time = qm->receive_time;
+    debug_queue_add(&sq->old_receive, qm);
+
+    pthread_mutex_unlock(&sq->lock);
+    return;
+
+exit:
+    pqm->len = -1;
+    pthread_mutex_unlock(&sq->lock);
+}
+
+// Set the estimated clock rate of the mcu on the other end of the
+// serial port
+void
+serialqueue_set_clock_est(struct serialqueue *sq, double est_clock
+                          , double last_ack_time, uint64_t last_ack_clock)
+{
+    pthread_mutex_lock(&sq->lock);
+    sq->est_clock = est_clock;
+    sq->last_ack_time = last_ack_time;
+    sq->last_ack_clock = last_ack_clock;
+    pthread_mutex_unlock(&sq->lock);
+}
+
+// Flush all messages in a "ready" state
+void
+serialqueue_flush_ready(struct serialqueue *sq)
+{
+    pthread_mutex_lock(&sq->lock);
+    sq->can_delay_writes = 0;
+    pthread_mutex_unlock(&sq->lock);
+    kick_bg_thread(sq);
+}
+
+// Return a string buffer containing statistics for the serial port
+void
+serialqueue_get_stats(struct serialqueue *sq, char *buf, int len)
+{
+    struct serialqueue stats;
+    pthread_mutex_lock(&sq->lock);
+    memcpy(&stats, sq, sizeof(stats));
+    pthread_mutex_unlock(&sq->lock);
+
+    snprintf(buf, len, "bytes_write=%u bytes_read=%u"
+             " bytes_retransmit=%u bytes_invalid=%u"
+             " send_seq=%u receive_seq=%u retransmit_seq=%u"
+             " srtt=%.3f rttvar=%.3f rto=%.3f"
+             " ready_bytes=%u stalled_bytes=%u"
+             , stats.bytes_write, stats.bytes_read
+             , stats.bytes_retransmit, stats.bytes_invalid
+             , (int)stats.send_seq, (int)stats.receive_seq
+             , (int)stats.retransmit_seq
+             , stats.srtt, stats.rttvar, stats.rto
+             , stats.ready_bytes, stats.stalled_bytes);
+}
+
+// Extract old messages stored in the debug queues
+int
+serialqueue_extract_old(struct serialqueue *sq, int sentq
+                        , struct pull_queue_message *q, int max)
+{
+    int count = sentq ? DEBUG_QUEUE_SENT : DEBUG_QUEUE_RECEIVE;
+    struct list_head *rootp = sentq ? &sq->old_sent : &sq->old_receive;
+    struct list_head replacement, current;
+    list_init(&replacement);
+    debug_queue_alloc(&replacement, count);
+    list_init(&current);
+
+    // Atomically replace existing debug list with new zero'd list
+    pthread_mutex_lock(&sq->lock);
+    list_join_tail(rootp, &current);
+    list_init(rootp);
+    list_join_tail(&replacement, rootp);
+    pthread_mutex_unlock(&sq->lock);
+
+    // Walk the debug list
+    int pos = 0;
+    while (!list_empty(&current) && pos < max) {
+        struct queue_message *qm = list_first_entry(
+            &current, struct queue_message, node);
+        if (qm->len) {
+            struct pull_queue_message *pqm = q++;
+            pos++;
+            memcpy(pqm->msg, qm->msg, qm->len);
+            pqm->len = qm->len;
+            pqm->sent_time = qm->sent_time;
+            pqm->receive_time = qm->receive_time;
+        }
+        list_del(&qm->node);
+        message_free(qm);
+    }
+    return pos;
+}
diff --git a/klippy/serialqueue.h b/klippy/serialqueue.h
new file mode 100644
index 00000000..229fb216
--- /dev/null
+++ b/klippy/serialqueue.h
@@ -0,0 +1,66 @@
+#ifndef SERIALQUEUE_H
+#define SERIALQUEUE_H
+
+#include "list.h" // struct list_head
+
+#define MAX_CLOCK 0x7fffffffffffffff
+
+#define MESSAGE_MIN 5
+#define MESSAGE_MAX 64
+#define MESSAGE_HEADER_SIZE  2
+#define MESSAGE_TRAILER_SIZE 3
+#define MESSAGE_POS_LEN 0
+#define MESSAGE_POS_SEQ 1
+#define MESSAGE_TRAILER_CRC  3
+#define MESSAGE_TRAILER_SYNC 1
+#define MESSAGE_PAYLOAD_MAX (MESSAGE_MAX - MESSAGE_MIN)
+#define MESSAGE_SEQ_MASK 0x0f
+#define MESSAGE_DEST 0x10
+#define MESSAGE_SYNC 0x7E
+
+struct queue_message {
+    int len;
+    uint8_t msg[MESSAGE_MAX];
+    union {
+        // Filled when on a command queue
+        struct {
+            uint64_t min_clock, req_clock;
+        };
+        // Filled when in sent/receive queues
+        struct {
+            double sent_time, receive_time;
+        };
+    };
+    struct list_node node;
+};
+
+struct queue_message *message_alloc_and_encode(uint32_t *data, int len);
+
+struct pull_queue_message {
+    uint8_t msg[MESSAGE_MAX];
+    int len;
+    double sent_time, receive_time;
+};
+
+struct serialqueue;
+struct serialqueue *serialqueue_alloc(int serial_fd, double baud_adjust
+                                      , int write_only);
+void serialqueue_exit(struct serialqueue *sq);
+struct command_queue *serialqueue_alloc_commandqueue(void);
+void serialqueue_send_batch(struct serialqueue *sq, struct command_queue *cq
+                            , struct list_head *msgs);
+void serialqueue_send(struct serialqueue *sq, struct command_queue *cq
+                      , uint8_t *msg, int len
+                      , uint64_t min_clock, uint64_t req_clock);
+void serialqueue_encode_and_send(struct serialqueue *sq, struct command_queue *cq
+                                 , uint32_t *data, int len
+                                 , uint64_t min_clock, uint64_t req_clock);
+void serialqueue_pull(struct serialqueue *sq, struct pull_queue_message *pqm);
+void serialqueue_set_clock_est(struct serialqueue *sq, double est_clock
+                               , double last_ack_time, uint64_t last_ack_clock);
+void serialqueue_flush_ready(struct serialqueue *sq);
+void serialqueue_get_stats(struct serialqueue *sq, char *buf, int len);
+int serialqueue_extract_old(struct serialqueue *sq, int sentq
+                            , struct pull_queue_message *q, int max);
+
+#endif // serialqueue.h
diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c
new file mode 100644
index 00000000..d39bbc32
--- /dev/null
+++ b/klippy/stepcompress.c
@@ -0,0 +1,498 @@
+// Stepper pulse schedule compression
+//
+// Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+//
+// The goal of this code is to take a series of scheduled stepper
+// pulse times and compress them into a handful of commands that can
+// be efficiently transmitted and executed on a microcontroller (mcu).
+// The mcu accepts step pulse commands that take interval, count, and
+// add parameters such that 'count' pulses occur, with each step event
+// calculating the next step event time using:
+//  next_wake_time = last_wake_time + interval; interval += add
+// This code is writtin in C (instead of python) for processing
+// efficiency - the repetitive integer math is vastly faster in C.
+
+#include <math.h> // sqrt
+#include <stddef.h> // offsetof
+#include <stdint.h> // uint32_t
+#include <stdio.h> // fprintf
+#include <stdlib.h> // malloc
+#include <string.h> // memset
+#include "serialqueue.h" // struct queue_message
+
+#define CHECK_LINES 1
+#define QUEUE_START_SIZE 1024
+
+struct stepcompress {
+    // Buffer management
+    uint32_t *queue, *queue_end, *queue_pos, *queue_next;
+    // Internal tracking
+    uint32_t relclock, max_error;
+    // Error checking
+    uint32_t errors;
+    // Message generation
+    uint64_t last_step_clock;
+    struct list_head msg_queue;
+    uint32_t queue_step_msgid, oid;
+};
+
+
+/****************************************************************
+ * Queue management
+ ****************************************************************/
+
+// Shuffle the internal queue to avoid having to allocate more ram
+static void
+clean_queue(struct stepcompress *sc)
+{
+    uint32_t *src = sc->queue_pos, *dest = sc->queue;
+    while (src < sc->queue_next)
+        *dest++ = *src++ - sc->relclock;
+    sc->queue_pos = sc->queue;
+    sc->queue_next = dest;
+    sc->relclock = 0;
+}
+
+// Expand the internal queue of step times
+static void
+expand_queue(struct stepcompress *sc, int count)
+{
+    if (sc->queue + count <= sc->queue_end) {
+        clean_queue(sc);
+        return;
+    }
+    int alloc = sc->queue_end - sc->queue;
+    int pos = sc->queue_pos - sc->queue;
+    int next = sc->queue_next - sc->queue;
+    if (!alloc)
+        alloc = QUEUE_START_SIZE;
+    while (next + count > alloc)
+        alloc *= 2;
+    sc->queue = realloc(sc->queue, alloc * sizeof(*sc->queue));
+    sc->queue_end = sc->queue + alloc;
+    sc->queue_pos = sc->queue + pos;
+    sc->queue_next = sc->queue + next;
+}
+
+// Check if the internal queue needs to be expanded, and expand if so
+static inline void
+check_expand(struct stepcompress *sc, int count)
+{
+    if (sc->queue_next + count > sc->queue_end)
+        expand_queue(sc, count);
+}
+
+
+/****************************************************************
+ * Step compression
+ ****************************************************************/
+
+#define DIV_UP(n,d) (((n) + (d) - 1) / (d))
+
+static inline int32_t
+idiv_up(int32_t n, int32_t d)
+{
+    return (n>=0) ? DIV_UP(n,d) : (n/d);
+}
+
+static inline int32_t
+idiv_down(int32_t n, int32_t d)
+{
+    return (n>=0) ? (n/d) : (n - d + 1) / d;
+}
+
+struct points {
+    int32_t minp, maxp;
+};
+
+// Given a requested step time, return the minimum and maximum
+// acceptable times
+static struct points
+minmax_point(struct stepcompress *sc, uint32_t *pos)
+{
+    uint32_t prevpoint = pos > sc->queue_pos ? *(pos-1) - sc->relclock : 0;
+    uint32_t point = *pos - sc->relclock;
+    uint32_t max_error = (point - prevpoint) / 2;
+    if (max_error > sc->max_error)
+        max_error = sc->max_error;
+    return (struct points){ point - max_error, point };
+}
+
+// The maximum add delta between two valid quadratic sequences of the
+// form "add*count*(count-1)/2 + interval*count" is "(6 + 4*sqrt(2)) *
+// maxerror / (count*count)".  The "6 + 4*sqrt(2)" is 11.65685, but
+// using 11 and rounding up when dividing works well in practice.
+#define QUADRATIC_DEV 11
+
+struct step_move {
+    uint32_t interval;
+    uint16_t count;
+    int16_t add;
+};
+
+// Find a 'step_move' that covers a series of step times
+static struct step_move
+compress_bisect_add(struct stepcompress *sc)
+{
+    uint32_t *last = sc->queue_next;
+    if (last > sc->queue_pos + 65535)
+        last = sc->queue_pos + 65535;
+    struct points point = minmax_point(sc, sc->queue_pos);
+    int32_t origmininterval = point.minp, origmaxinterval = point.maxp;
+    int32_t add = 0, minadd=-0x8001, maxadd=0x8000;
+    int32_t bestadd=0, bestcount=0, bestinterval=0;
+
+    for (;;) {
+        // Find longest valid sequence with the given 'add'
+        int32_t mininterval = origmininterval, maxinterval = origmaxinterval;
+        int32_t count = 1, addfactor = 0;
+        for (;;) {
+            if (sc->queue_pos + count >= last)
+                return (struct step_move){ maxinterval, count, add };
+            point = minmax_point(sc, sc->queue_pos + count);
+            addfactor += count;
+            int32_t c = add*addfactor;
+            int32_t nextmininterval = mininterval;
+            if (c + nextmininterval*(count+1) < point.minp)
+                nextmininterval = DIV_UP(point.minp - c, count+1);
+            int32_t nextmaxinterval = maxinterval;
+            if (c + nextmaxinterval*(count+1) > point.maxp)
+                nextmaxinterval = (point.maxp - c) / (count+1);
+            if (nextmininterval > nextmaxinterval)
+                break;
+            count += 1;
+            mininterval = nextmininterval;
+            maxinterval = nextmaxinterval;
+        }
+        if (count > bestcount || (count == bestcount && add > bestadd)) {
+            bestcount = count;
+            bestadd = add;
+            bestinterval = maxinterval;
+        }
+
+        // Check if a greater or lesser add could extend the sequence
+        int32_t maxreach = add*addfactor + maxinterval*(count+1);
+        if (maxreach < point.minp)
+            origmaxinterval = maxinterval;
+        else
+            origmininterval = mininterval;
+
+        if ((minadd+1)*addfactor + origmaxinterval*(count+1) < point.minp)
+            minadd = idiv_up(point.minp - origmaxinterval*(count+1)
+                             , addfactor) - 1;
+        if ((maxadd-1)*addfactor + origmininterval*(count+1) > point.maxp)
+            maxadd = idiv_down(point.maxp - origmininterval*(count+1)
+                               , addfactor) + 1;
+
+        // The maximum valid deviation between two quadratic sequences
+        // can be calculated and used to further limit the add range.
+        if (count > 1) {
+            int32_t errdelta = DIV_UP(sc->max_error*QUADRATIC_DEV, count*count);
+            if (minadd < add - errdelta)
+                minadd = add - errdelta;
+            if (maxadd > add + errdelta)
+                maxadd = add + errdelta;
+        }
+
+        // Bisect valid add range and try again with new 'add'
+        add = (maxadd + minadd) / 2;
+        if (add <= minadd || add >= maxadd)
+            break;
+    }
+    if (bestcount < 2)
+        bestadd = 0;
+    return (struct step_move){ bestinterval, bestcount, bestadd };
+}
+
+
+/****************************************************************
+ * Step compress checking
+ ****************************************************************/
+
+// Verify that a given 'step_move' matches the actual step times
+static void
+check_line(struct stepcompress *sc, struct step_move move)
+{
+    if (!CHECK_LINES)
+        return;
+    int err = 0;
+    if (!move.count || !move.interval || move.interval >= 0x80000000) {
+        fprintf(stderr, "ERROR: Point out of range: %d %d %d\n"
+                , move.interval, move.count, move.add);
+        err++;
+    }
+    uint32_t interval = move.interval, p = interval;
+    uint16_t i;
+    for (i=0; i<move.count; i++) {
+        struct points point = minmax_point(sc, sc->queue_pos + i);
+        if (p < point.minp || p > point.maxp) {
+            fprintf(stderr, "ERROR: Point %d of %d: %d not in %d:%d\n"
+                    , i+1, move.count, p, point.minp, point.maxp);
+            err++;
+        }
+        interval += move.add;
+        p += interval;
+    }
+    sc->errors += err;
+}
+
+
+/****************************************************************
+ * Step compress interface
+ ****************************************************************/
+
+// Allocate a new 'stepcompress' object
+struct stepcompress *
+stepcompress_alloc(uint32_t max_error, uint32_t queue_step_msgid, uint32_t oid)
+{
+    struct stepcompress *sc = malloc(sizeof(*sc));
+    memset(sc, 0, sizeof(*sc));
+    sc->max_error = max_error;
+    list_init(&sc->msg_queue);
+    sc->queue_step_msgid = queue_step_msgid;
+    sc->oid = oid;
+    return sc;
+}
+
+// Schedule a step event at the specified step_clock time
+void
+stepcompress_push(struct stepcompress *sc, double step_clock)
+{
+    check_expand(sc, 1);
+    step_clock += 0.5 + sc->relclock - sc->last_step_clock;
+    *sc->queue_next++ = step_clock;
+}
+
+// Schedule 'steps' number of steps with a constant time between steps
+// using the formula: step_clock = clock_offset + step_num*factor
+double
+stepcompress_push_factor(struct stepcompress *sc
+                         , double steps, double step_offset
+                         , double clock_offset, double factor)
+{
+    // Calculate number of steps to take
+    double ceil_steps = ceil(steps - step_offset);
+    double next_step_offset = ceil_steps - (steps - step_offset);
+    int count = ceil_steps;
+    check_expand(sc, count);
+
+    // Calculate each step time
+    uint32_t *qn = sc->queue_next, *end = &qn[count];
+    clock_offset += 0.5 + sc->relclock - sc->last_step_clock;
+    double pos = step_offset;
+    while (qn < end) {
+        *qn++ = clock_offset + pos*factor;
+        pos += 1.0;
+    }
+    sc->queue_next = qn;
+    return next_step_offset;
+}
+
+// Schedule 'steps' number of steps using the formula:
+//  step_clock = clock_offset + sqrt(step_num*factor + sqrt_offset)
+double
+stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset
+                       , double clock_offset, double sqrt_offset, double factor)
+{
+    // Calculate number of steps to take
+    double ceil_steps = ceil(steps - step_offset);
+    double next_step_offset = ceil_steps - (steps - step_offset);
+    int count = ceil_steps;
+    check_expand(sc, count);
+
+    // Calculate each step time
+    uint32_t *qn = sc->queue_next, *end = &qn[count];
+    clock_offset += 0.5 + sc->relclock - sc->last_step_clock;
+    double pos = step_offset + sqrt_offset/factor;
+    if (factor >= 0.0)
+        while (qn < end) {
+            *qn++ = clock_offset + sqrt(pos*factor);
+            pos += 1.0;
+        }
+    else
+        while (qn < end) {
+            *qn++ = clock_offset - sqrt(pos*factor);
+            pos += 1.0;
+        }
+    sc->queue_next = end;
+    return next_step_offset;
+}
+
+// Convert previously scheduled steps into commands for the mcu
+static void
+stepcompress_flush(struct stepcompress *sc, uint64_t move_clock)
+{
+    if (sc->queue_pos >= sc->queue_next)
+        return;
+    while (move_clock > sc->last_step_clock) {
+        struct step_move move = compress_bisect_add(sc);
+        check_line(sc, move);
+
+        uint32_t msg[5] = {
+            sc->queue_step_msgid, sc->oid, move.interval, move.count, move.add
+        };
+        struct queue_message *qm = message_alloc_and_encode(msg, 5);
+        qm->req_clock = sc->last_step_clock;
+        list_add_tail(&qm->node, &sc->msg_queue);
+
+        uint32_t addfactor = move.count*(move.count-1)/2;
+        uint32_t ticks = move.add*addfactor + move.interval*move.count;
+        sc->last_step_clock += ticks;
+        if (sc->queue_pos + move.count >= sc->queue_next) {
+            sc->queue_pos = sc->queue_next = sc->queue;
+            sc->relclock = 0;
+            break;
+        }
+        sc->queue_pos += move.count;
+        sc->relclock += ticks;
+    }
+}
+
+// Reset the internal state of the stepcompress object
+void
+stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock)
+{
+    stepcompress_flush(sc, UINT64_MAX);
+    sc->last_step_clock = last_step_clock;
+}
+
+// Queue an mcu command to go out in order with stepper commands
+void
+stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len)
+{
+    stepcompress_flush(sc, UINT64_MAX);
+
+    struct queue_message *qm = message_alloc_and_encode(data, len);
+    qm->min_clock = -1;
+    qm->req_clock = sc->last_step_clock;
+    list_add_tail(&qm->node, &sc->msg_queue);
+}
+
+// Return the count of internal errors found
+uint32_t
+stepcompress_get_errors(struct stepcompress *sc)
+{
+    return sc->errors;
+}
+
+
+/****************************************************************
+ * Step compress synchronization
+ ****************************************************************/
+
+// The steppersync object is used to synchronize the output of mcu
+// step commands.  The mcu can only queue a limited number of step
+// commands - this code tracks when items on the mcu step queue become
+// free so that new commands can be transmitted.  It also ensures the
+// mcu step queue is ordered between steppers so that no stepper
+// starves the other steppers of space in the mcu step queue.
+
+struct steppersync {
+    // Serial port
+    struct serialqueue *sq;
+    struct command_queue *cq;
+    // Storage for associated stepcompress objects
+    struct stepcompress **sc_list;
+    int sc_num;
+    // Storage for list of pending move clocks
+    uint64_t *move_clocks;
+    int num_move_clocks;
+};
+
+// Allocate a new 'stepperysync' object
+struct steppersync *
+steppersync_alloc(struct serialqueue *sq, struct stepcompress **sc_list
+                  , int sc_num, int move_num)
+{
+    struct steppersync *ss = malloc(sizeof(*ss));
+    memset(ss, 0, sizeof(*ss));
+    ss->sq = sq;
+    ss->cq = serialqueue_alloc_commandqueue();
+
+    ss->sc_list = malloc(sizeof(*sc_list)*sc_num);
+    memcpy(ss->sc_list, sc_list, sizeof(*sc_list)*sc_num);
+    ss->sc_num = sc_num;
+
+    ss->move_clocks = malloc(sizeof(*ss->move_clocks)*move_num);
+    memset(ss->move_clocks, 0, sizeof(*ss->move_clocks)*move_num);
+    ss->num_move_clocks = move_num;
+
+    return ss;
+}
+
+// Implement a binary heap algorithm to track when the next available
+// 'struct move' in the mcu will be available
+static void
+heap_replace(struct steppersync *ss, uint64_t req_clock)
+{
+    uint64_t *mc = ss->move_clocks;
+    int nmc = ss->num_move_clocks, pos = 0;
+    for (;;) {
+        int child1_pos = 2*pos+1, child2_pos = 2*pos+2;
+        uint64_t child2_clock = child2_pos < nmc ? mc[child2_pos] : UINT64_MAX;
+        uint64_t child1_clock = child1_pos < nmc ? mc[child1_pos] : UINT64_MAX;
+        if (req_clock <= child1_clock && req_clock <= child2_clock) {
+            mc[pos] = req_clock;
+            break;
+        }
+        if (child1_clock < child2_clock) {
+            mc[pos] = child1_clock;
+            pos = child1_pos;
+        } else {
+            mc[pos] = child2_clock;
+            pos = child2_pos;
+        }
+    }
+}
+
+// Find and transmit any scheduled steps prior to the given 'move_clock'
+void
+steppersync_flush(struct steppersync *ss, uint64_t move_clock)
+{
+    // Flush each stepcompress to the specified move_clock
+    int i;
+    for (i=0; i<ss->sc_num; i++)
+        stepcompress_flush(ss->sc_list[i], move_clock);
+
+    // Order commands by the reqclock of each pending command
+    struct list_head msgs;
+    list_init(&msgs);
+    uint64_t min_clock = ss->move_clocks[0];
+    for (;;) {
+        // Find message with lowest reqclock
+        uint64_t req_clock = MAX_CLOCK;
+        struct queue_message *qm = NULL;
+        for (i=0; i<ss->sc_num; i++) {
+            struct stepcompress *sc = ss->sc_list[i];
+            if (!list_empty(&sc->msg_queue)) {
+                struct queue_message *m = list_first_entry(
+                    &sc->msg_queue, struct queue_message, node);
+                if (m->req_clock < req_clock) {
+                    qm = m;
+                    req_clock = m->req_clock;
+                }
+            }
+        }
+        if (!qm || (!qm->min_clock && req_clock > move_clock))
+            break;
+
+        // Set the min_clock for this command
+        if (!qm->min_clock) {
+            qm->min_clock = min_clock;
+            heap_replace(ss, req_clock);
+            min_clock = ss->move_clocks[0];
+        } else {
+            qm->min_clock = min_clock;
+        }
+
+        // Batch this command
+        list_del(&qm->node);
+        list_add_tail(&qm->node, &msgs);
+    }
+
+    // Transmit commands
+    if (!list_empty(&msgs))
+        serialqueue_send_batch(ss->sq, ss->cq, &msgs);
+}
diff --git a/klippy/stepper.py b/klippy/stepper.py
new file mode 100644
index 00000000..61faaee0
--- /dev/null
+++ b/klippy/stepper.py
@@ -0,0 +1,67 @@
+# Printer stepper support
+#
+# Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import math, logging
+
+class PrinterStepper:
+    def __init__(self, printer, config):
+        self.printer = printer
+        self.config = config
+        self.mcu_stepper = self.mcu_enable = self.mcu_endstop = None
+
+        self.step_dist = config.getfloat('step_distance')
+        self.inv_step_dist = 1. / self.step_dist
+        max_velocity = config.getfloat('max_velocity')
+        self.max_step_velocity = max_velocity * self.inv_step_dist
+        max_accel = config.getfloat('max_accel')
+        self.max_step_accel = max_accel * self.inv_step_dist
+
+        self.homing_speed = config.getfloat('homing_speed', 5.0)
+        self.homing_positive_dir = config.getboolean(
+            'homing_positive_dir', False)
+        self.homing_retract_dist = config.getfloat('homing_retract_dist', 5.)
+        self.position_min = config.getfloat('position_min', 0.)
+        self.position_endstop = config.getfloat('position_endstop')
+        self.position_max = config.getfloat('position_max')
+
+        self.clock_ticks = None
+        self.need_motor_enable = True
+    def build_config(self):
+        self.clock_ticks = self.printer.mcu.get_mcu_freq()
+        max_error = self.config.getfloat('max_error', 0.000050)
+        max_error = int(max_error * self.clock_ticks)
+
+        step_pin = self.config.get('step_pin')
+        dir_pin = self.config.get('dir_pin')
+        jc = 0.005 # XXX
+        min_stop_interval = int((math.sqrt(1./self.max_step_accel + jc**2) - jc)
+                                * self.clock_ticks) - max_error
+        min_stop_interval = max(0, min_stop_interval)
+        mcu = self.printer.mcu
+        self.mcu_stepper = mcu.create_stepper(
+            step_pin, dir_pin, min_stop_interval, max_error)
+        enable_pin = self.config.get('enable_pin')
+        if enable_pin is not None:
+            self.mcu_enable = mcu.create_digital_out(enable_pin, 0)
+        endstop_pin = self.config.get('endstop_pin')
+        if endstop_pin is not None:
+            self.mcu_endstop = mcu.create_endstop(endstop_pin, self.mcu_stepper)
+    def motor_enable(self, move_time, enable=0):
+        if (self.mcu_enable is not None
+            and self.mcu_enable.get_last_setting() != enable):
+            mc = int(self.mcu_enable.get_print_clock(move_time))
+            self.mcu_enable.set_digital(mc + 1, enable)
+        self.need_motor_enable = True
+    def prep_move(self, sdir, move_time):
+        move_clock = self.mcu_stepper.get_print_clock(move_time)
+        self.mcu_stepper.set_next_step_dir(sdir, int(move_clock))
+        if self.need_motor_enable:
+            self.motor_enable(move_time, 1)
+            self.need_motor_enable = False
+        return (move_clock, self.clock_ticks, self.mcu_stepper)
+    def enable_endstop_checking(self, move_time, hz):
+        move_clock = int(self.mcu_endstop.get_print_clock(move_time))
+        self.mcu_endstop.home(move_clock, int(self.clock_ticks / hz))
+        return self.mcu_endstop
diff --git a/klippy/util.py b/klippy/util.py
new file mode 100644
index 00000000..caac827e
--- /dev/null
+++ b/klippy/util.py
@@ -0,0 +1,32 @@
+# Low level unix utility functions
+#
+# Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+
+import os, pty, fcntl, termios, signal
+
+# Return the SIGINT interrupt handler back to the OS default
+def fix_sigint():
+    signal.signal(signal.SIGINT, signal.SIG_DFL)
+fix_sigint()
+
+# Set a file-descriptor as non-blocking
+def set_nonblock(fd):
+    fcntl.fcntl(fd, fcntl.F_SETFL
+                , fcntl.fcntl(fd, fcntl.F_GETFL) | os.O_NONBLOCK)
+
+# Support for creating a pseudo-tty for emulating a serial port
+def create_pty(ptyname):
+    mfd, sfd = pty.openpty()
+    try:
+        os.unlink(ptyname)
+    except os.error:
+        pass
+    os.symlink(os.ttyname(sfd), ptyname)
+    fcntl.fcntl(mfd, fcntl.F_SETFL
+                , fcntl.fcntl(mfd, fcntl.F_GETFL) | os.O_NONBLOCK)
+    old = termios.tcgetattr(mfd)
+    old[3] = old[3] & ~termios.ECHO
+    termios.tcsetattr(mfd, termios.TCSADRAIN, old)
+    return mfd