Initial commit of source code.

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

36 files changed, 3129 insertions, 0 deletions

diff --git a/src/Kconfig b/src/Kconfig
new file mode 100644
index 00000000..e7460cce
--- /dev/null
+++ b/src/Kconfig
@@ -0,0 +1,20 @@
+# Main Kconfig settings
+
+mainmenu "Klipper Firmware Configuration"
+
+choice
+    prompt "Micro-controller Architecture"
+    config MACH_AVR
+        bool "Atmega AVR"
+    config MACH_SIMU
+        bool "Host simulator"
+endchoice
+
+source "src/avr/Kconfig"
+source "src/simulator/Kconfig"
+
+config INLINE_STEPPER_HACK
+    # Enables gcc to inline stepper_event() into the main timer irq handler
+    bool
+    default y if MACH_AVR
+    default n
diff --git a/src/avr/Kconfig b/src/avr/Kconfig
new file mode 100644
index 00000000..e0964676
--- /dev/null
+++ b/src/avr/Kconfig
@@ -0,0 +1,64 @@
+# Kconfig settings for AVR processors
+
+if MACH_AVR
+
+config BOARD_DIRECTORY
+    string
+    default "avr"
+
+choice
+    prompt "Processor model"
+    config MACH_atmega168
+        bool "atmega168"
+    config MACH_atmega644p
+        bool "atmega644p"
+    config MACH_atmega1280
+        bool "atmega1280"
+    config MACH_atmega2560
+        bool "atmega2560"
+endchoice
+
+config MCU
+    string
+    default "atmega168" if MACH_atmega168
+    default "atmega644p" if MACH_atmega644p
+    default "atmega1280" if MACH_atmega1280
+    default "atmega2560" if MACH_atmega2560
+
+choice
+    prompt "Processor speed"
+    config AVR_FREQ_8000000
+        bool "8Mhz"
+    config AVR_FREQ_16000000
+        bool "16Mhz"
+    config AVR_FREQ_20000000
+        bool "20Mhz"
+endchoice
+
+config CLOCK_FREQ
+    int
+    default 8000000 if AVR_FREQ_8000000
+    default 16000000 if AVR_FREQ_16000000
+    default 20000000 if AVR_FREQ_20000000
+
+config AVR_STACK_SIZE
+    int
+    default 256 if MACH_atmega2560
+    default 128
+
+config AVR_WATCHDOG
+    bool "Support for automated reset on watchdog timeout"
+    default y
+config AVR_SERIAL
+    bool
+    default y
+config SERIAL_BAUD
+    depends on AVR_SERIAL
+    int "Baud rate for serial port"
+    default 250000
+config SERIAL_BAUD_U2X
+    depends on AVR_SERIAL
+    bool "Use AVR Baud 2X mode"
+    default y
+
+endif
diff --git a/src/avr/Makefile b/src/avr/Makefile
new file mode 100644
index 00000000..758443da
--- /dev/null
+++ b/src/avr/Makefile
@@ -0,0 +1,19 @@
+# Additional avr build rules
+
+# Use the avr toolchain
+CROSS_PREFIX=avr-
+
+CFLAGS-y += -mmcu=$(CONFIG_MCU) -DF_CPU=$(CONFIG_CLOCK_FREQ)
+LDFLAGS-y += -Wl,--relax
+
+# Add avr source files
+src-y += avr/main.c avr/timer.c avr/gpio.c avr/alloc.c
+src-$(CONFIG_AVR_WATCHDOG) += avr/watchdog.c
+src-$(CONFIG_AVR_SERIAL) += avr/serial.c
+
+# Build the additional hex output file
+target-y += $(OUT)klipper.elf.hex
+
+$(OUT)klipper.elf.hex: $(OUT)klipper.elf
+	@echo "  Creating hex file $@"
+	$(Q)$(OBJCOPY) -j .text -j .data -O ihex $< $@
diff --git a/src/avr/alloc.c b/src/avr/alloc.c
new file mode 100644
index 00000000..aaa671bc
--- /dev/null
+++ b/src/avr/alloc.c
@@ -0,0 +1,25 @@
+// AVR allocation checking code.
+//
+// Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <avr/io.h> // AVR_STACK_POINTER_REG
+#include <stdlib.h> // __malloc_heap_end
+#include "autoconf.h" // CONFIG_AVR_STACK_SIZE
+#include "compiler.h" // ALIGN
+#include "misc.h" // alloc_maxsize
+
+size_t
+alloc_maxsize(size_t reqsize)
+{
+    uint16_t memend = ALIGN(AVR_STACK_POINTER_REG, 256);
+    __malloc_heap_end = (void*)memend - CONFIG_AVR_STACK_SIZE;
+    extern char *__brkval;
+    int16_t maxsize = __malloc_heap_end - __brkval - 2;
+    if (maxsize < 0)
+        return 0;
+    if (reqsize < maxsize)
+        return reqsize;
+    return maxsize;
+}
diff --git a/src/avr/gpio.c b/src/avr/gpio.c
new file mode 100644
index 00000000..d2cdfbd8
--- /dev/null
+++ b/src/avr/gpio.c
@@ -0,0 +1,337 @@
+// GPIO functions on AVR.
+//
+// Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <stddef.h> // NULL
+#include "autoconf.h" // CONFIG_MACH_atmega644p
+#include "command.h" // shutdown
+#include "gpio.h" // gpio_out_write
+#include "irq.h" // irq_save
+#include "pgm.h" // PROGMEM
+#include "sched.h" // DECL_INIT
+
+
+/****************************************************************
+ * AVR chip definitions
+ ****************************************************************/
+
+#define GPIO(PORT, NUM) (((PORT)-'A') * 8 + (NUM))
+#define GPIO2PORT(PIN) ((PIN) / 8)
+#define GPIO2BIT(PIN) (1<<((PIN) % 8))
+
+static volatile uint8_t * const digital_regs[] PROGMEM = {
+#ifdef PINA
+    &PINA,
+#else
+    NULL,
+#endif
+    &PINB, &PINC, &PIND,
+#ifdef PINE
+    &PINE, &PINF, &PING, &PINH, NULL, &PINJ, &PINK, &PINL
+#endif
+};
+
+struct gpio_digital_regs {
+    // gcc (pre v6) does better optimization when uint8_t are bitfields
+    volatile uint8_t in : 8, mode : 8, out : 8;
+};
+
+#define GPIO2REGS(pin)                                                  \
+    ((struct gpio_digital_regs*)READP(digital_regs[GPIO2PORT(pin)]))
+
+struct gpio_pwm_info {
+    volatile void *ocr;
+    volatile uint8_t *rega, *regb;
+    uint8_t en_bit, pin, flags;
+};
+
+enum { GP_8BIT=1, GP_AFMT=2 };
+
+static const struct gpio_pwm_info pwm_regs[] PROGMEM = {
+#if CONFIG_MACH_atmega168
+    { &OCR0A, &TCCR0A, &TCCR0B, 1<<COM0A1, GPIO('D', 6), GP_8BIT         },
+    { &OCR0B, &TCCR0A, &TCCR0B, 1<<COM0B1, GPIO('D', 5), GP_8BIT         },
+//    { &OCR1A, &TCCR1A, &TCCR1B, 1<<COM1A1, GPIO('B', 1), 0               },
+//    { &OCR1B, &TCCR1A, &TCCR1B, 1<<COM1B1, GPIO('B', 2), 0               },
+    { &OCR2A, &TCCR2A, &TCCR2B, 1<<COM2A1, GPIO('B', 3), GP_8BIT|GP_AFMT },
+    { &OCR2B, &TCCR2A, &TCCR2B, 1<<COM2B1, GPIO('D', 3), GP_8BIT|GP_AFMT },
+#elif CONFIG_MACH_atmega644p
+    { &OCR0A, &TCCR0A, &TCCR0B, 1<<COM0A1, GPIO('B', 3), GP_8BIT         },
+    { &OCR0B, &TCCR0A, &TCCR0B, 1<<COM0B1, GPIO('B', 4), GP_8BIT         },
+//    { &OCR1A, &TCCR1A, &TCCR1B, 1<<COM1A1, GPIO('D', 5), 0               },
+//    { &OCR1B, &TCCR1A, &TCCR1B, 1<<COM1B1, GPIO('D', 4), 0               },
+    { &OCR2A, &TCCR2A, &TCCR2B, 1<<COM2A1, GPIO('D', 7), GP_8BIT|GP_AFMT },
+    { &OCR2B, &TCCR2A, &TCCR2B, 1<<COM2B1, GPIO('D', 6), GP_8BIT|GP_AFMT },
+#elif CONFIG_MACH_atmega1280 || CONFIG_MACH_atmega2560
+    { &OCR0A, &TCCR0A, &TCCR0B, 1<<COM0A1, GPIO('B', 7), GP_8BIT         },
+    { &OCR0B, &TCCR0A, &TCCR0B, 1<<COM0B1, GPIO('G', 5), GP_8BIT         },
+//    { &OCR1A, &TCCR1A, &TCCR1B, 1<<COM1A1, GPIO('B', 5), 0               },
+//    { &OCR1B, &TCCR1A, &TCCR1B, 1<<COM1B1, GPIO('B', 6), 0               },
+//    { &OCR1C, &TCCR1A, &TCCR1B, 1<<COM1C1, GPIO('B', 7), 0               },
+    { &OCR2A, &TCCR2A, &TCCR2B, 1<<COM2A1, GPIO('B', 4), GP_8BIT|GP_AFMT },
+    { &OCR2B, &TCCR2A, &TCCR2B, 1<<COM2B1, GPIO('H', 6), GP_8BIT|GP_AFMT },
+    { &OCR3A, &TCCR3A, &TCCR3B, 1<<COM3A1, GPIO('E', 3), 0               },
+    { &OCR3B, &TCCR3A, &TCCR3B, 1<<COM3B1, GPIO('E', 4), 0               },
+    { &OCR3C, &TCCR3A, &TCCR3B, 1<<COM3C1, GPIO('E', 5), 0               },
+    { &OCR4A, &TCCR4A, &TCCR4B, 1<<COM4A1, GPIO('H', 3), 0               },
+    { &OCR4B, &TCCR4A, &TCCR4B, 1<<COM4B1, GPIO('H', 4), 0               },
+    { &OCR4C, &TCCR4A, &TCCR4B, 1<<COM4C1, GPIO('H', 5), 0               },
+    { &OCR5A, &TCCR5A, &TCCR5B, 1<<COM5A1, GPIO('L', 3), 0               },
+    { &OCR5B, &TCCR5A, &TCCR5B, 1<<COM5B1, GPIO('L', 4), 0               },
+    { &OCR5C, &TCCR5A, &TCCR5B, 1<<COM5C1, GPIO('L', 5), 0               },
+#endif
+};
+
+struct gpio_adc_info {
+    uint8_t pin;
+};
+
+static const struct gpio_adc_info adc_pins[] PROGMEM = {
+#if CONFIG_MACH_atmega168
+    { GPIO('C', 0) }, { GPIO('C', 1) }, { GPIO('C', 2) }, { GPIO('C', 3) },
+    { GPIO('C', 4) }, { GPIO('C', 5) }, { GPIO('E', 0) }, { GPIO('E', 1) },
+#elif CONFIG_MACH_atmega644p
+    { GPIO('A', 0) }, { GPIO('A', 1) }, { GPIO('A', 2) }, { GPIO('A', 3) },
+    { GPIO('A', 4) }, { GPIO('A', 5) }, { GPIO('A', 6) }, { GPIO('A', 7) },
+#elif CONFIG_MACH_atmega1280 || CONFIG_MACH_atmega2560
+    { GPIO('F', 0) }, { GPIO('F', 1) }, { GPIO('F', 2) }, { GPIO('F', 3) },
+    { GPIO('F', 4) }, { GPIO('F', 5) }, { GPIO('F', 6) }, { GPIO('F', 7) },
+    { GPIO('K', 0) }, { GPIO('K', 1) }, { GPIO('K', 2) }, { GPIO('K', 3) },
+    { GPIO('K', 4) }, { GPIO('K', 5) }, { GPIO('K', 6) }, { GPIO('K', 7) },
+#endif
+};
+
+#if CONFIG_MACH_atmega168
+static const uint8_t SS = GPIO('B', 2), SCK = GPIO('B', 5), MOSI = GPIO('B', 3);
+#elif CONFIG_MACH_atmega644p
+static const uint8_t SS = GPIO('B', 4), SCK = GPIO('B', 7), MOSI = GPIO('B', 5);
+#elif CONFIG_MACH_atmega1280 || CONFIG_MACH_atmega2560
+static const uint8_t SS = GPIO('B', 0), SCK = GPIO('B', 1), MOSI = GPIO('B', 2);
+#endif
+
+static const uint8_t ADMUX_DEFAULT = 0x40;
+
+
+/****************************************************************
+ * gpio functions
+ ****************************************************************/
+
+struct gpio_out
+gpio_out_setup(uint8_t pin, uint8_t val)
+{
+    if (GPIO2PORT(pin) > ARRAY_SIZE(digital_regs))
+        goto fail;
+    struct gpio_digital_regs *regs = GPIO2REGS(pin);
+    if (! regs)
+        goto fail;
+    uint8_t bit = GPIO2BIT(pin);
+    uint8_t flag = irq_save();
+    regs->out = val ? (regs->out | bit) : (regs->out & ~bit);
+    regs->mode |= bit;
+    irq_restore(flag);
+    return (struct gpio_out){ .regs=regs, .bit=bit };
+fail:
+    shutdown("Not an output pin");
+}
+
+void gpio_out_toggle(struct gpio_out g)
+{
+    g.regs->in = g.bit;
+}
+
+void
+gpio_out_write(struct gpio_out g, uint8_t val)
+{
+    uint8_t flag = irq_save();
+    g.regs->out = val ? (g.regs->out | g.bit) : (g.regs->out & ~g.bit);
+    irq_restore(flag);
+}
+
+struct gpio_in
+gpio_in_setup(uint8_t pin, int8_t pull_up)
+{
+    if (GPIO2PORT(pin) > ARRAY_SIZE(digital_regs))
+        goto fail;
+    struct gpio_digital_regs *regs = GPIO2REGS(pin);
+    if (! regs)
+        goto fail;
+    uint8_t bit = GPIO2BIT(pin);
+    uint8_t flag = irq_save();
+    regs->out = pull_up > 0 ? (regs->out | bit) : (regs->out & ~bit);
+    regs->mode &= ~bit;
+    irq_restore(flag);
+    return (struct gpio_in){ .regs=regs, .bit=bit };
+fail:
+    shutdown("Not an input pin");
+}
+
+uint8_t
+gpio_in_read(struct gpio_in g)
+{
+    return !!(g.regs->in & g.bit);
+}
+
+
+void
+gpio_pwm_write(struct gpio_pwm g, uint8_t val)
+{
+    if (g.size8) {
+        *(volatile uint8_t*)g.reg = val;
+    } else {
+        uint8_t flag = irq_save();
+        *(volatile uint16_t*)g.reg = val;
+        irq_restore(flag);
+    }
+}
+
+struct gpio_pwm
+gpio_pwm_setup(uint8_t pin, uint32_t cycle_time, uint8_t val)
+{
+    uint8_t chan;
+    for (chan=0; chan<ARRAY_SIZE(pwm_regs); chan++) {
+        const struct gpio_pwm_info *p = &pwm_regs[chan];
+        if (READP(p->pin) != pin)
+            continue;
+        uint8_t flags = READP(p->flags), cs;
+        if (flags & GP_AFMT) {
+            switch (cycle_time) {
+            case 0        ...    8*510L - 1: cs = 1; break;
+            case 8*510L   ...   32*510L - 1: cs = 2; break;
+            case 32*510L  ...   64*510L - 1: cs = 3; break;
+            case 64*510L  ...  128*510L - 1: cs = 4; break;
+            case 128*510L ...  256*510L - 1: cs = 5; break;
+            case 256*510L ... 1024*510L - 1: cs = 6; break;
+            default:                         cs = 7; break;
+            }
+        } else {
+            switch (cycle_time) {
+            case 0        ...    8*510L - 1: cs = 1; break;
+            case 8*510L   ...   64*510L - 1: cs = 2; break;
+            case 64*510L  ...  256*510L - 1: cs = 3; break;
+            case 256*510L ... 1024*510L - 1: cs = 4; break;
+            default:                         cs = 5; break;
+            }
+        }
+        volatile uint8_t *rega = READP(p->rega), *regb = READP(p->regb);
+        uint8_t en_bit = READP(p->en_bit);
+        struct gpio_digital_regs *regs = GPIO2REGS(pin);
+        uint8_t bit = GPIO2BIT(pin);
+        struct gpio_pwm g = (struct gpio_pwm) {
+            (void*)READP(p->ocr), flags & GP_8BIT };
+
+        // Setup PWM timer
+        uint8_t flag = irq_save();
+        uint8_t old_cs = *regb & 0x07;
+        if (old_cs && old_cs != cs)
+            shutdown("PWM already programmed at different speed");
+        *regb = cs;
+
+        // Set default value and enable output
+        gpio_pwm_write(g, val);
+        *rega |= (1<<WGM00) | en_bit;
+        regs->mode |= bit;
+        irq_restore(flag);
+
+        return g;
+    }
+    shutdown("Not a valid PWM pin");
+}
+
+
+struct gpio_adc
+gpio_adc_setup(uint8_t pin)
+{
+    uint8_t chan;
+    for (chan=0; chan<ARRAY_SIZE(adc_pins); chan++) {
+        const struct gpio_adc_info *a = &adc_pins[chan];
+        if (READP(a->pin) != pin)
+            continue;
+
+        // Enable ADC
+        ADCSRA |= (1<<ADPS0)|(1<<ADPS1)|(1<<ADPS2)|(1<<ADEN);
+
+        // Disable digital input for this pin
+#ifdef DIDR2
+        if (chan >= 8)
+            DIDR2 |= 1 << (chan & 0x07);
+        else
+#endif
+            DIDR0 |= 1 << chan;
+
+        return (struct gpio_adc){ chan };
+    }
+    shutdown("Not a valid ADC pin");
+}
+
+uint32_t
+gpio_adc_sample_time(void)
+{
+    return (13 + 1) * 128 + 200;
+}
+
+enum { ADC_DUMMY=0xff };
+static uint8_t last_analog_read = ADC_DUMMY;
+
+uint8_t
+gpio_adc_sample(struct gpio_adc g)
+{
+    if (ADCSRA & (1<<ADSC))
+        // Busy
+        return 1;
+    if (last_analog_read == g.chan)
+        // Sample now ready
+        return 0;
+    if (last_analog_read != ADC_DUMMY)
+        // Sample on another channel in progress
+        return 1;
+    last_analog_read = g.chan;
+
+#if defined(ADCSRB) && defined(MUX5)
+    // the MUX5 bit of ADCSRB selects whether we're reading from channels
+    // 0 to 7 (MUX5 low) or 8 to 15 (MUX5 high).
+    ADCSRB = (ADCSRB & ~(1 << MUX5)) | (((g.chan >> 3) & 0x01) << MUX5);
+#endif
+
+    ADMUX = ADMUX_DEFAULT | (g.chan & 0x07);
+
+    // start the conversion
+    ADCSRA |= 1<<ADSC;
+    return 1;
+}
+
+void
+gpio_adc_clear_sample(struct gpio_adc g)
+{
+    if (last_analog_read == g.chan)
+        last_analog_read = ADC_DUMMY;
+}
+
+uint16_t
+gpio_adc_read(struct gpio_adc g)
+{
+    last_analog_read = ADC_DUMMY;
+    return ADC;
+}
+
+
+void
+spi_config(void)
+{
+    gpio_out_setup(SS, 1);
+    gpio_out_setup(SCK, 0);
+    gpio_out_setup(MOSI, 0);
+    SPCR = (1<<MSTR) | (1<<SPE);
+}
+
+void
+spi_transfer(char *data, uint8_t len)
+{
+    while (len--) {
+        SPDR = *data;
+        while (!(SPSR & (1<<SPIF)))
+            ;
+        *data++ = SPDR;
+    }
+}
diff --git a/src/avr/gpio.h b/src/avr/gpio.h
new file mode 100644
index 00000000..e6cbbd32
--- /dev/null
+++ b/src/avr/gpio.h
@@ -0,0 +1,42 @@
+#ifndef __AVR_GPIO_H
+#define __AVR_GPIO_H
+
+#include <stdint.h>
+#include "compiler.h" // __always_inline
+
+struct gpio_out {
+    struct gpio_digital_regs *regs;
+    // gcc (pre v6) does better optimization when uint8_t are bitfields
+    uint8_t bit : 8;
+};
+struct gpio_out gpio_out_setup(uint8_t pin, uint8_t val);
+void gpio_out_toggle(struct gpio_out g);
+void gpio_out_write(struct gpio_out g, uint8_t val);
+
+struct gpio_in {
+    struct gpio_digital_regs *regs;
+    uint8_t bit;
+};
+struct gpio_in gpio_in_setup(uint8_t pin, int8_t pull_up);
+uint8_t gpio_in_read(struct gpio_in g);
+
+struct gpio_pwm {
+    void *reg;
+    uint8_t size8;
+};
+struct gpio_pwm gpio_pwm_setup(uint8_t pin, uint32_t cycle_time, uint8_t val);
+void gpio_pwm_write(struct gpio_pwm g, uint8_t val);
+
+struct gpio_adc {
+    uint8_t chan;
+};
+struct gpio_adc gpio_adc_setup(uint8_t pin);
+uint32_t gpio_adc_sample_time(void);
+uint8_t gpio_adc_sample(struct gpio_adc g);
+void gpio_adc_clear_sample(struct gpio_adc g);
+uint16_t gpio_adc_read(struct gpio_adc g);
+
+void spi_config(void);
+void spi_transfer(char *data, uint8_t len);
+
+#endif // gpio.h
diff --git a/src/avr/irq.h b/src/avr/irq.h
new file mode 100644
index 00000000..bfc6cb51
--- /dev/null
+++ b/src/avr/irq.h
@@ -0,0 +1,29 @@
+#ifndef __AVR_IRQ_H
+#define __AVR_IRQ_H
+// Definitions for irq enable/disable on AVR
+
+#include <avr/interrupt.h> // cli
+#include "compiler.h" // barrier
+
+static inline void irq_disable(void) {
+    cli();
+    barrier();
+}
+
+static inline void irq_enable(void) {
+    barrier();
+    sei();
+}
+
+static inline uint8_t irq_save(void) {
+    uint8_t flag = SREG;
+    irq_disable();
+    return flag;
+}
+
+static inline void irq_restore(uint8_t flag) {
+    barrier();
+    SREG = flag;
+}
+
+#endif // irq.h
diff --git a/src/avr/main.c b/src/avr/main.c
new file mode 100644
index 00000000..7651ab4a
--- /dev/null
+++ b/src/avr/main.c
@@ -0,0 +1,17 @@
+// Main starting point for AVR boards.
+//
+// Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include "irq.h" // irq_enable
+#include "sched.h" // sched_main
+
+// Main entry point for avr code.
+int
+main(void)
+{
+    irq_enable();
+    sched_main();
+    return 0;
+}
diff --git a/src/avr/misc.h b/src/avr/misc.h
new file mode 100644
index 00000000..244cd5bc
--- /dev/null
+++ b/src/avr/misc.h
@@ -0,0 +1,25 @@
+#ifndef __AVR_MISC_H
+#define __AVR_MISC_H
+
+#include <stdint.h>
+#include <util/crc16.h>
+
+// alloc.c
+size_t alloc_maxsize(size_t reqsize);
+
+// console.c
+char *console_get_input(uint8_t *plen);
+void console_pop_input(uint8_t len);
+char *console_get_output(uint8_t len);
+void console_push_output(uint8_t len);
+
+// Optimized crc16_ccitt for the avr processor
+#define HAVE_OPTIMIZED_CRC 1
+static inline uint16_t _crc16_ccitt(char *buf, uint8_t len) {
+    uint16_t crc = 0xFFFF;
+    while (len--)
+        crc = _crc_ccitt_update(crc, *buf++);
+    return crc;
+}
+
+#endif // misc.h
diff --git a/src/avr/pgm.h b/src/avr/pgm.h
new file mode 100644
index 00000000..ba68d8f9
--- /dev/null
+++ b/src/avr/pgm.h
@@ -0,0 +1,25 @@
+#ifndef __AVR_PGM_H
+#define __AVR_PGM_H
+// This header provides the avr/pgmspace.h definitions for "PROGMEM"
+// on AVR platforms.
+
+#include <avr/pgmspace.h>
+
+#define READP(VAR) ({                                                   \
+    _Pragma("GCC diagnostic push");                                     \
+    _Pragma("GCC diagnostic ignored \"-Wint-to-pointer-cast\"");        \
+    typeof(VAR) __val =                                                 \
+        __builtin_choose_expr(sizeof(VAR) == 1,                         \
+            (typeof(VAR))pgm_read_byte(&(VAR)),                         \
+        __builtin_choose_expr(sizeof(VAR) == 2,                         \
+            (typeof(VAR))pgm_read_word(&(VAR)),                         \
+        __builtin_choose_expr(sizeof(VAR) == 4,                         \
+            (typeof(VAR))pgm_read_dword(&(VAR)),                        \
+        __force_link_error__unknown_type)));                            \
+    _Pragma("GCC diagnostic pop");                                      \
+    __val;                                                              \
+    })
+
+extern void __force_link_error__unknown_type(void);
+
+#endif // pgm.h
diff --git a/src/avr/serial.c b/src/avr/serial.c
new file mode 100644
index 00000000..c73890dd
--- /dev/null
+++ b/src/avr/serial.c
@@ -0,0 +1,137 @@
+// AVR serial port code.
+//
+// Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <avr/interrupt.h> // USART0_RX_vect
+#include <string.h> // memmove
+#include "autoconf.h" // CONFIG_SERIAL_BAUD
+#include "sched.h" // DECL_INIT
+#include "irq.h" // irq_save
+#include "misc.h" // console_get_input
+
+#define SERIAL_BUFFER_SIZE 96
+static char receive_buf[SERIAL_BUFFER_SIZE];
+static uint8_t receive_pos;
+static char transmit_buf[SERIAL_BUFFER_SIZE];
+static uint8_t transmit_pos, transmit_max;
+
+
+/****************************************************************
+ * Serial hardware
+ ****************************************************************/
+
+static void
+serial_init(void)
+{
+    if (CONFIG_SERIAL_BAUD_U2X) {
+        UCSR0A = 1<<U2X0;
+        UBRR0 = DIV_ROUND_CLOSEST(F_CPU, 8UL * CONFIG_SERIAL_BAUD) - 1UL;
+    } else {
+        UCSR0A = 0;
+        UBRR0 = DIV_ROUND_CLOSEST(F_CPU, 16UL * CONFIG_SERIAL_BAUD) - 1UL;
+    }
+
+    UCSR0C = (1<<UCSZ01) | (1<<UCSZ00);
+    UCSR0B = (1<<RXEN0) | (1<<TXEN0) | (1<<RXCIE0) | (1<<UDRIE0);
+}
+DECL_INIT(serial_init);
+
+#ifdef USART_RX_vect
+#define USART0_RX_vect USART_RX_vect
+#define USART0_UDRE_vect USART_UDRE_vect
+#endif
+
+// Rx interrupt - data available to be read.
+ISR(USART0_RX_vect)
+{
+    uint8_t data = UDR0;
+    if (receive_pos >= sizeof(receive_buf))
+        // Serial overflow - ignore it as crc error will force retransmit
+        return;
+    receive_buf[receive_pos++] = data;
+}
+
+// Tx interrupt - data can be written to serial.
+ISR(USART0_UDRE_vect)
+{
+    if (transmit_pos >= transmit_max)
+        UCSR0B &= ~(1<<UDRIE0);
+    else
+        UDR0 = transmit_buf[transmit_pos++];
+}
+
+
+/****************************************************************
+ * Console access functions
+ ****************************************************************/
+
+// Return a buffer (and length) containing any incoming messages
+char *
+console_get_input(uint8_t *plen)
+{
+    *plen = readb(&receive_pos);
+    return receive_buf;
+}
+
+// Remove from the receive buffer the given number of bytes
+void
+console_pop_input(uint8_t len)
+{
+    uint8_t copied = 0;
+    for (;;) {
+        uint8_t rpos = readb(&receive_pos);
+        uint8_t needcopy = rpos - len;
+        if (needcopy) {
+            memmove(&receive_buf[copied], &receive_buf[copied + len]
+                    , needcopy - copied);
+            copied = needcopy;
+        }
+        uint8_t flag = irq_save();
+        if (rpos != readb(&receive_pos)) {
+            // Raced with irq handler - retry
+            irq_restore(flag);
+            continue;
+        }
+        receive_pos = needcopy;
+        irq_restore(flag);
+        break;
+    }
+}
+
+// Return an output buffer that the caller may fill with transmit messages
+char *
+console_get_output(uint8_t len)
+{
+    uint8_t tpos = readb(&transmit_pos), tmax = readb(&transmit_max);
+    if (tpos == tmax) {
+        tpos = tmax = 0;
+        writeb(&transmit_max, 0);
+        writeb(&transmit_pos, 0);
+    }
+    if (tmax + len <= sizeof(transmit_buf))
+        return &transmit_buf[tmax];
+    if (tmax - tpos + len > sizeof(transmit_buf))
+        return NULL;
+    // Disable TX irq and move buffer
+    writeb(&transmit_max, 0);
+    barrier();
+    tpos = readb(&transmit_pos);
+    tmax -= tpos;
+    memmove(&transmit_buf[0], &transmit_buf[tpos], tmax);
+    writeb(&transmit_pos, 0);
+    barrier();
+    writeb(&transmit_max, tmax);
+    UCSR0B |= 1<<UDRIE0;
+    return &transmit_buf[tmax];
+}
+
+// Accept the given number of bytes added to the transmit buffer
+void
+console_push_output(uint8_t len)
+{
+    writeb(&transmit_max, readb(&transmit_max) + len);
+    // enable TX interrupt
+    UCSR0B |= 1<<UDRIE0;
+}
diff --git a/src/avr/timer.c b/src/avr/timer.c
new file mode 100644
index 00000000..0b248552
--- /dev/null
+++ b/src/avr/timer.c
@@ -0,0 +1,171 @@
+// AVR timer interrupt scheduling code.
+//
+// Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <avr/interrupt.h> // TCNT1
+#include "command.h" // shutdown
+#include "irq.h" // irq_save
+#include "sched.h" // sched_timer_kick
+#include "timer.h" // timer_from_ms
+
+
+/****************************************************************
+ * Low level timer code
+ ****************************************************************/
+
+// Return the number of clock ticks for a given number of milliseconds
+uint32_t
+timer_from_ms(uint32_t ms)
+{
+    return ms * (F_CPU / 1000);
+}
+
+static inline uint16_t
+timer_get(void)
+{
+    return TCNT1;
+}
+
+static inline void
+timer_set(uint16_t next)
+{
+    OCR1A = next;
+}
+
+static inline void
+timer_set_clear(uint16_t next)
+{
+    OCR1A = next;
+    TIFR1 = 1<<OCF1A;
+}
+
+ISR(TIMER1_COMPA_vect)
+{
+    sched_timer_kick();
+}
+
+static void
+timer_init(void)
+{
+    // no outputs
+    TCCR1A = 0;
+    // Normal Mode
+    TCCR1B = 1<<CS10;
+    // enable interrupt
+    TIMSK1 = 1<<OCIE1A;
+}
+DECL_INIT(timer_init);
+
+
+/****************************************************************
+ * 32bit timer wrappers
+ ****************************************************************/
+
+static uint32_t timer_last;
+
+// Return the 32bit current time given the 16bit current time.
+static __always_inline uint32_t
+calc_time(uint32_t last, uint16_t cur)
+{
+    union u32_u16_u calc;
+    calc.val = last;
+    if (cur < calc.lo)
+        calc.hi++;
+    calc.lo = cur;
+    return calc.val;
+}
+
+// Called by main code once every millisecond.  (IRQs disabled.)
+void
+timer_periodic(void)
+{
+    timer_last = calc_time(timer_last, timer_get());
+}
+
+// Return the current time (in absolute clock ticks).
+uint32_t
+timer_read_time(void)
+{
+    uint8_t flag = irq_save();
+    uint16_t cur = timer_get();
+    uint32_t last = timer_last;
+    irq_restore(flag);
+    return calc_time(last, cur);
+}
+
+#define TIMER_MIN_TICKS 100
+
+// Set the next timer wake time (in absolute clock ticks).  Caller
+// must disable irqs.  The caller should not schedule a time more than
+// a few milliseconds in the future.
+uint8_t
+timer_set_next(uint32_t next)
+{
+    uint16_t cur = timer_get();
+    if ((int16_t)(OCR1A - cur) < 0 && !(TIFR1 & (1<<OCF1A)))
+        // Already processing timer irqs
+        try_shutdown("timer_set_next called during timer dispatch");
+    uint32_t mintime = calc_time(timer_last, cur + TIMER_MIN_TICKS);
+    if (sched_is_before(mintime, next)) {
+        timer_set_clear(next);
+        return 0;
+    }
+    timer_set_clear(mintime);
+    return 1;
+}
+
+static uint8_t timer_repeat;
+#define TIMER_MAX_REPEAT 40
+#define TIMER_MAX_NEXT_REPEAT 15
+
+#define TIMER_MIN_TRY_TICKS 60 // 40 ticks to exit irq; 20 ticks of progress
+#define TIMER_DEFER_REPEAT_TICKS 200
+
+// Similar to timer_set_next(), but wait for the given time if it is
+// in the near future.
+uint8_t
+timer_try_set_next(uint32_t target)
+{
+    uint16_t next = target, now = timer_get();
+    int16_t diff = next - now;
+    if (diff > TIMER_MIN_TRY_TICKS)
+        // Schedule next timer normally.
+        goto done;
+
+    // Next timer is in the past or near future - can't reschedule to it
+    uint8_t tr = timer_repeat-1;
+    if (likely(tr)) {
+        irq_enable();
+        timer_repeat = tr;
+        irq_disable();
+        while (diff >= 0) {
+            // Next timer is in the near future - wait for time to occur
+            now = timer_get();
+            irq_enable();
+            diff = next - now;
+            irq_disable();
+        }
+        return 0;
+    }
+
+    // Too many repeat timers from a single interrupt - force a pause
+    timer_repeat = TIMER_MAX_NEXT_REPEAT;
+    next = now + TIMER_DEFER_REPEAT_TICKS;
+    if (diff < (int16_t)(-timer_from_ms(1)))
+        goto fail;
+
+done:
+    timer_set(next);
+    return 1;
+fail:
+    shutdown("Rescheduled timer in the past");
+}
+
+static void
+timer_task(void)
+{
+    timer_repeat = TIMER_MAX_REPEAT;
+}
+DECL_TASK(timer_task);
diff --git a/src/avr/timer.h b/src/avr/timer.h
new file mode 100644
index 00000000..2165fecd
--- /dev/null
+++ b/src/avr/timer.h
@@ -0,0 +1,12 @@
+#ifndef __AVR_TIMER_H
+#define __AVR_TIMER_H
+
+#include <stdint.h>
+
+uint32_t timer_from_ms(uint32_t ms);
+void timer_periodic(void);
+uint32_t timer_read_time(void);
+uint8_t timer_set_next(uint32_t next);
+uint8_t timer_try_set_next(uint32_t next);
+
+#endif // timer.h
diff --git a/src/avr/watchdog.c b/src/avr/watchdog.c
new file mode 100644
index 00000000..f925a8d5
--- /dev/null
+++ b/src/avr/watchdog.c
@@ -0,0 +1,38 @@
+// Initialization of AVR watchdog timer.
+//
+// Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <avr/interrupt.h> // WDT_vect
+#include <avr/wdt.h> // wdt_enable
+#include "command.h" // shutdown
+#include "sched.h" // DECL_TASK
+
+static uint8_t watchdog_shutdown;
+
+ISR(WDT_vect)
+{
+    watchdog_shutdown = 1;
+    shutdown("Watchdog timer!");
+}
+
+static void
+watchdog_reset(void)
+{
+    wdt_reset();
+    if (watchdog_shutdown) {
+        WDTCSR |= 1<<WDIE;
+        watchdog_shutdown = 0;
+    }
+}
+DECL_TASK(watchdog_reset);
+
+static void
+watchdog_init(void)
+{
+    // 0.5s timeout, interrupt and system reset
+    wdt_enable(WDTO_500MS);
+    WDTCSR |= 1<<WDIE;
+}
+DECL_INIT(watchdog_init);
diff --git a/src/basecmd.c b/src/basecmd.c
new file mode 100644
index 00000000..b94c820a
--- /dev/null
+++ b/src/basecmd.c
@@ -0,0 +1,301 @@
+// Basic infrastructure commands.
+//
+// Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <stdlib.h> // malloc
+#include <string.h> // memcpy
+#include "basecmd.h" // lookup_oid
+#include "board/irq.h" // irq_save
+#include "board/misc.h" // alloc_maxsize
+#include "command.h" // DECL_COMMAND
+#include "sched.h" // sched_clear_shutdown
+
+
+/****************************************************************
+ * Move queue
+ ****************************************************************/
+
+static struct move *move_list, *move_free_list;
+static uint16_t move_count;
+
+void
+move_free(struct move *m)
+{
+    m->next = move_free_list;
+    move_free_list = m;
+}
+
+struct move *
+move_alloc(void)
+{
+    uint8_t flag = irq_save();
+    struct move *m = move_free_list;
+    if (!m)
+        shutdown("Move queue empty");
+    move_free_list = m->next;
+    irq_restore(flag);
+    return m;
+}
+
+static void
+move_reset(void)
+{
+    if (!move_count)
+        return;
+    // Add everything in move_list to the free list.
+    uint32_t i;
+    for (i=0; i<move_count-1; i++)
+        move_list[i].next = &move_list[i+1];
+    move_list[move_count-1].next = NULL;
+    move_free_list = &move_list[0];
+}
+DECL_SHUTDOWN(move_reset);
+
+
+/****************************************************************
+ * Generic object ids (oid)
+ ****************************************************************/
+
+struct oid_s {
+    void *type, *data;
+};
+
+static struct oid_s *oids;
+static uint8_t num_oid;
+static uint32_t config_crc;
+static uint8_t config_finalized;
+
+void *
+lookup_oid(uint8_t oid, void *type)
+{
+    if (oid >= num_oid || type != oids[oid].type)
+        shutdown("Invalid oid type");
+    return oids[oid].data;
+}
+
+static void
+assign_oid(uint8_t oid, void *type, void *data)
+{
+    if (oid >= num_oid || oids[oid].type || config_finalized)
+        shutdown("Can't assign oid");
+    oids[oid].type = type;
+    oids[oid].data = data;
+}
+
+void *
+alloc_oid(uint8_t oid, void *type, uint16_t size)
+{
+    void *data = malloc(size);
+    if (!data)
+        shutdown("malloc failed");
+    memset(data, 0, size);
+    assign_oid(oid, type, data);
+    return data;
+}
+
+void *
+next_oid(uint8_t *i, void *type)
+{
+    uint8_t oid = *i;
+    for (;;) {
+        oid++;
+        if (oid >= num_oid)
+            return NULL;
+        if (oids[oid].type == type) {
+            *i = oid;
+            return oids[oid].data;
+        }
+    }
+}
+
+void
+command_allocate_oids(uint32_t *args)
+{
+    if (oids)
+        shutdown("oids already allocated");
+    uint8_t count = args[0];
+    oids = malloc(sizeof(oids[0]) * count);
+    if (!oids)
+        shutdown("malloc failed");
+    memset(oids, 0, sizeof(oids[0]) * count);
+    num_oid = count;
+}
+DECL_COMMAND(command_allocate_oids, "allocate_oids count=%c");
+
+void
+command_get_config(uint32_t *args)
+{
+    sendf("config is_config=%c crc=%u move_count=%hu"
+          , config_finalized, config_crc, move_count);
+}
+DECL_COMMAND_FLAGS(command_get_config, HF_IN_SHUTDOWN, "get_config");
+
+void
+command_finalize_config(uint32_t *args)
+{
+    if (!oids || config_finalized)
+        shutdown("Can't finalize");
+    uint16_t count = alloc_maxsize(sizeof(*move_list)*1024) / sizeof(*move_list);
+    move_list = malloc(count * sizeof(*move_list));
+    if (!count || !move_list)
+        shutdown("malloc failed");
+    move_count = count;
+    move_reset();
+    config_crc = args[0];
+    config_finalized = 1;
+    command_get_config(NULL);
+}
+DECL_COMMAND(command_finalize_config, "finalize_config crc=%u");
+
+
+/****************************************************************
+ * Group commands
+ ****************************************************************/
+
+static struct timer group_timer;
+
+static uint8_t
+group_end_event(struct timer *timer)
+{
+    shutdown("Missed scheduling of next event");
+}
+
+void
+command_start_group(uint32_t *args)
+{
+    sched_del_timer(&group_timer);
+    group_timer.func = group_end_event;
+    group_timer.waketime = args[0];
+    sched_timer(&group_timer);
+}
+DECL_COMMAND(command_start_group, "start_group clock=%u");
+
+void
+command_end_group(uint32_t *args)
+{
+    sched_del_timer(&group_timer);
+}
+DECL_COMMAND(command_end_group, "end_group");
+
+
+/****************************************************************
+ * Timing and load stats
+ ****************************************************************/
+
+void
+command_get_status(uint32_t *args)
+{
+    sendf("status clock=%u status=%c", sched_read_time(), sched_is_shutdown());
+}
+DECL_COMMAND_FLAGS(command_get_status, HF_IN_SHUTDOWN, "get_status");
+
+static void
+stats_task(void)
+{
+    static uint32_t last, count, sumsq;
+    uint32_t cur = sched_read_time();
+    uint32_t diff = (cur - last) >> 8;
+    last = cur;
+    count++;
+    uint32_t nextsumsq = sumsq + diff*diff;
+    if (nextsumsq < sumsq)
+        nextsumsq = 0xffffffff;
+    sumsq = nextsumsq;
+
+    static uint32_t prev;
+    if (sched_is_before(cur, prev + sched_from_ms(5000)))
+        return;
+    sendf("stats count=%u sum=%u sumsq=%u", count, cur - prev, sumsq);
+    prev = cur;
+    count = sumsq = 0;
+}
+DECL_TASK(stats_task);
+
+
+/****************************************************************
+ * Register debug commands
+ ****************************************************************/
+
+void
+command_debug_read8(uint32_t *args)
+{
+    uint8_t *ptr = (void*)(size_t)args[0];
+    uint16_t v = *ptr;
+    sendf("debug_result val=%hu", v);
+}
+DECL_COMMAND_FLAGS(command_debug_read8, HF_IN_SHUTDOWN, "debug_read8 addr=%u");
+
+void
+command_debug_read16(uint32_t *args)
+{
+    uint16_t *ptr = (void*)(size_t)args[0];
+    uint8_t flag = irq_save();
+    uint16_t v = *ptr;
+    irq_restore(flag);
+    sendf("debug_result val=%hu", v);
+}
+DECL_COMMAND_FLAGS(command_debug_read16, HF_IN_SHUTDOWN, "debug_read16 addr=%u");
+
+void
+command_debug_write8(uint32_t *args)
+{
+    uint8_t *ptr = (void*)(size_t)args[0];
+    *ptr = args[1];
+}
+DECL_COMMAND_FLAGS(command_debug_write8, HF_IN_SHUTDOWN,
+                   "debug_write8 addr=%u val=%u");
+
+void
+command_debug_write16(uint32_t *args)
+{
+    uint16_t *ptr = (void*)(size_t)args[0];
+    uint8_t flag = irq_save();
+    *ptr = args[1];
+    irq_restore(flag);
+}
+DECL_COMMAND_FLAGS(command_debug_write16, HF_IN_SHUTDOWN,
+                   "debug_write16 addr=%u val=%u");
+
+
+/****************************************************************
+ * Misc commands
+ ****************************************************************/
+
+void
+command_reset(uint32_t *args)
+{
+    // XXX - implement reset
+}
+DECL_COMMAND_FLAGS(command_reset, HF_IN_SHUTDOWN, "msg_reset");
+
+void
+command_emergency_stop(uint32_t *args)
+{
+    shutdown("command request");
+}
+DECL_COMMAND_FLAGS(command_emergency_stop, HF_IN_SHUTDOWN, "emergency_stop");
+
+void
+command_clear_shutdown(uint32_t *args)
+{
+    sched_clear_shutdown();
+}
+DECL_COMMAND_FLAGS(command_clear_shutdown, HF_IN_SHUTDOWN, "clear_shutdown");
+
+void
+command_identify(uint32_t *args)
+{
+    uint32_t offset = args[0];
+    uint8_t count = args[1];
+    uint32_t isize = READP(command_identify_size);
+    if (offset >= isize)
+        count = 0;
+    else if (offset + count > isize)
+        count = isize - offset;
+    sendf("identify_response offset=%u data=%.*s"
+          , offset, count, &command_identify_data[offset]);
+}
+DECL_COMMAND_FLAGS(command_identify, HF_IN_SHUTDOWN,
+                   "identify offset=%u count=%c");
diff --git a/src/basecmd.h b/src/basecmd.h
new file mode 100644
index 00000000..e5719c6f
--- /dev/null
+++ b/src/basecmd.h
@@ -0,0 +1,23 @@
+#ifndef __BASECMD_H
+#define __BASECMD_H
+
+#include <stdint.h> // uint8_t
+
+struct move {
+    uint32_t interval;
+    int16_t add;
+    uint16_t count;
+    struct move *next;
+    uint8_t flags;
+};
+
+void move_free(struct move *m);
+struct move *move_alloc(void);
+void *lookup_oid(uint8_t oid, void *type);
+void *alloc_oid(uint8_t oid, void *type, uint16_t size);
+void *next_oid(uint8_t *i, void *type);
+
+#define foreach_oid(pos,data,oidtype)                   \
+    for (pos=-1; (data=next_oid(&pos, oidtype)); )
+
+#endif // basecmd.h
diff --git a/src/command.c b/src/command.c
new file mode 100644
index 00000000..8608fbdf
--- /dev/null
+++ b/src/command.c
@@ -0,0 +1,315 @@
+// Code for parsing incoming commands and encoding outgoing messages
+//
+// Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <ctype.h> // isspace
+#include <stdarg.h> // va_start
+#include <stdio.h> // vsnprintf
+#include <stdlib.h> // strtod
+#include <string.h> // strcasecmp
+#include "board/irq.h" // irq_disable
+#include "board/misc.h" // HAVE_OPTIMIZED_CRC
+#include "board/pgm.h" // READP
+#include "command.h" // output_P
+#include "sched.h" // DECL_TASK
+
+#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
+
+static uint8_t next_sequence = MESSAGE_DEST;
+
+
+/****************************************************************
+ * Binary message parsing
+ ****************************************************************/
+
+// Implement the standard crc "ccitt" algorithm on the given buffer
+static uint16_t
+crc16_ccitt(char *buf, uint8_t len)
+{
+    if (HAVE_OPTIMIZED_CRC)
+        return _crc16_ccitt(buf, 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;
+}
+
+// Encode an integer as a variable length quantity (vlq)
+static char *
+encode_int(char *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;
+}
+
+// Parse an integer that was encoded as a "variable length quantity"
+static uint32_t
+parse_int(char **pp)
+{
+    char *p = *pp;
+    uint8_t c = *p++;
+    uint32_t v = c & 0x7f;
+    if ((c & 0x60) == 0x60)
+        v |= -0x20;
+    while (c & 0x80) {
+        c = *p++;
+        v = (v<<7) | (c & 0x7f);
+    }
+    *pp = p;
+    return v;
+}
+
+// Parse an incoming command into 'args'
+static noinline char *
+parsef(char *p, char *maxend, const struct command_parser *cp, uint32_t *args)
+{
+    if (sched_is_shutdown() && !(READP(cp->flags) & HF_IN_SHUTDOWN)) {
+        sendf("is_shutdown static_string_id=%hu", sched_shutdown_reason());
+        return NULL;
+    }
+    uint8_t num_params = READP(cp->num_params);
+    const uint8_t *param_types = READP(cp->param_types);
+    while (num_params--) {
+        if (p > maxend)
+            goto error;
+        uint8_t t = READP(*param_types);
+        param_types++;
+        switch (t) {
+        case PT_uint32:
+        case PT_int32:
+        case PT_uint16:
+        case PT_int16:
+        case PT_byte:
+            *args++ = parse_int(&p);
+            break;
+        case PT_buffer: {
+            uint8_t len = *p++;
+            if (p + len > maxend)
+                goto error;
+            *args++ = len;
+            *args++ = (size_t)p;
+            p += len;
+            break;
+        }
+        default:
+            goto error;
+        }
+    }
+    return p;
+error:
+    shutdown("Command parser error");
+}
+
+// Encode a message and transmit it
+void
+_sendf(uint8_t parserid, ...)
+{
+    const struct command_encoder *cp = &command_encoders[parserid];
+    uint8_t max_size = READP(cp->max_size);
+    char *buf = console_get_output(max_size + MESSAGE_MIN);
+    if (!buf)
+        return;
+    char *p = &buf[MESSAGE_HEADER_SIZE];
+    if (max_size) {
+        char *maxend = &p[max_size];
+        va_list args;
+        va_start(args, parserid);
+        uint8_t num_params = READP(cp->num_params);
+        const uint8_t *param_types = READP(cp->param_types);
+        *p++ = READP(cp->msg_id);
+        while (num_params--) {
+            if (p > maxend)
+                goto error;
+            uint8_t t = READP(*param_types);
+            param_types++;
+            uint32_t v;
+            switch (t) {
+            case PT_uint32:
+            case PT_int32:
+            case PT_uint16:
+            case PT_int16:
+            case PT_byte:
+                if (t >= PT_uint16)
+                    v = va_arg(args, int) & 0xffff;
+                else
+                    v = va_arg(args, uint32_t);
+                p = encode_int(p, v);
+                break;
+            case PT_string: {
+                char *s = va_arg(args, char*), *lenp = p++;
+                while (*s && p<maxend)
+                    *p++ = *s++;
+                *lenp = p-lenp-1;
+                break;
+            }
+            case PT_progmem_buffer:
+            case PT_buffer: {
+                v = va_arg(args, int);
+                if (v > maxend-p)
+                    v = maxend-p;
+                *p++ = v;
+                char *s = va_arg(args, char*);
+                if (t == PT_progmem_buffer)
+                    memcpy_P(p, s, v);
+                else
+                    memcpy(p, s, v);
+                p += v;
+                break;
+            }
+            default:
+                goto error;
+            }
+        }
+        va_end(args);
+    }
+
+    // Send message to serial port
+    uint8_t msglen = p+MESSAGE_TRAILER_SIZE - buf;
+    buf[MESSAGE_POS_LEN] = msglen;
+    buf[MESSAGE_POS_SEQ] = next_sequence;
+    uint16_t crc = crc16_ccitt(buf, p-buf);
+    *p++ = crc>>8;
+    *p++ = crc;
+    *p++ = MESSAGE_SYNC;
+    console_push_output(msglen);
+    return;
+error:
+    shutdown("Message encode error");
+}
+
+
+/****************************************************************
+ * Command routing
+ ****************************************************************/
+
+// Find the command handler associated with a command
+static const struct command_parser *
+command_get_handler(uint8_t cmdid)
+{
+    if (cmdid >= READP(command_index_size))
+        goto error;
+    const struct command_parser *cp = READP(command_index[cmdid]);
+    if (!cp)
+        goto error;
+    return cp;
+error:
+    shutdown("Invalid command");
+}
+
+enum { CF_NEED_SYNC=1<<0, CF_NEED_VALID=1<<1 };
+
+// Find the next complete message.
+static char *
+command_get_message(void)
+{
+    static uint8_t sync_state;
+    uint8_t buf_len;
+    char *buf = console_get_input(&buf_len);
+    if (buf_len && sync_state & CF_NEED_SYNC)
+        goto need_sync;
+    if (buf_len < MESSAGE_MIN)
+        // Not ready to run.
+        return NULL;
+    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 NULL;
+    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;
+    sync_state &= ~CF_NEED_VALID;
+    // Check sequence number
+    if (msgseq != next_sequence) {
+        // Lost message - discard messages until it is retransmitted
+        console_pop_input(msglen);
+        goto nak;
+    }
+    next_sequence = ((msgseq + 1) & MESSAGE_SEQ_MASK) | MESSAGE_DEST;
+    sendf(""); // An empty message with a new sequence number is an ack
+    return buf;
+
+error:
+    if (buf[0] == MESSAGE_SYNC) {
+        // Ignore (do not nak) leading SYNC bytes
+        console_pop_input(1);
+        return NULL;
+    }
+    sync_state |= CF_NEED_SYNC;
+need_sync: ;
+    // Discard bytes until next SYNC found
+    char *next_sync = memchr(buf, MESSAGE_SYNC, buf_len);
+    if (next_sync) {
+        sync_state &= ~CF_NEED_SYNC;
+        console_pop_input(next_sync - buf + 1);
+    } else {
+        console_pop_input(buf_len);
+    }
+    if (sync_state & CF_NEED_VALID)
+        return NULL;
+    sync_state |= CF_NEED_VALID;
+nak:
+    sendf(""); // An empty message with a duplicate sequence number is a nak
+    return NULL;
+}
+
+// Background task that reads commands from the board serial port
+static void
+command_task(void)
+{
+    // Process commands.
+    char *buf = command_get_message();
+    if (!buf)
+        return;
+    uint8_t msglen = buf[MESSAGE_POS_LEN];
+    char *p = &buf[MESSAGE_HEADER_SIZE];
+    char *msgend = &buf[msglen-MESSAGE_TRAILER_SIZE];
+    while (p < msgend) {
+        uint8_t cmdid = *p++;
+        const struct command_parser *cp = command_get_handler(cmdid);
+        uint32_t args[READP(cp->num_args)];
+        p = parsef(p, msgend, cp, args);
+        if (!p)
+            break;
+        void (*func)(uint32_t*) = READP(cp->func);
+        func(args);
+    }
+    console_pop_input(msglen);
+    return;
+}
+DECL_TASK(command_task);
diff --git a/src/command.h b/src/command.h
new file mode 100644
index 00000000..2bbe37f8
--- /dev/null
+++ b/src/command.h
@@ -0,0 +1,81 @@
+#ifndef __COMMAND_H
+#define __COMMAND_H
+
+#include <stdarg.h> // va_list
+#include <stddef.h> // size_t
+#include <stdint.h> // uint8_t
+#include "compiler.h" // __section
+
+// Declare a function to run when the specified command is received
+#define DECL_COMMAND(FUNC, MSG)                 \
+    _DECL_COMMAND(FUNC, 0, MSG)
+#define DECL_COMMAND_FLAGS(FUNC, FLAGS, MSG)    \
+    _DECL_COMMAND(FUNC, FLAGS, MSG)
+
+// Flags for command handler declarations.
+#define HF_IN_SHUTDOWN   0x01   // Handler can run even when in emergency stop
+
+// Send an output message (and declare a static message type for it)
+#define output(FMT, args...)                    \
+    _sendf(_DECL_OUTPUT(FMT) , ##args )
+
+// Declare a message type and transmit it.
+#define sendf(FMT, args...)                     \
+    _sendf(_DECL_PARSER(FMT) , ##args)
+
+// Shut down the machine (also declares a static string to transmit)
+#define shutdown(msg)                           \
+    sched_shutdown(_DECL_STATIC_STR(msg))
+#define try_shutdown(msg)                       \
+    sched_try_shutdown(_DECL_STATIC_STR(msg))
+
+// command.c
+void _sendf(uint8_t parserid, ...);
+
+// out/compile_time_request.c (auto generated file)
+struct command_encoder {
+    uint8_t msg_id, max_size, num_params;
+    const uint8_t *param_types;
+};
+struct command_parser {
+    uint8_t msg_id, num_args, flags, num_params;
+    const uint8_t *param_types;
+    void (*func)(uint32_t *args);
+};
+enum {
+    PT_uint32, PT_int32, PT_uint16, PT_int16, PT_byte,
+    PT_string, PT_progmem_buffer, PT_buffer,
+};
+extern const struct command_encoder command_encoders[];
+extern const struct command_parser * const command_index[];
+extern const uint8_t command_index_size;
+extern const uint8_t command_identify_data[];
+extern const uint32_t command_identify_size;
+
+// Compiler glue for DECL_COMMAND macros above.
+#define _DECL_COMMAND(FUNC, FLAGS, MSG)                                 \
+    char __PASTE(_DECLS_ ## FUNC ## _, __LINE__) []                     \
+        __visible __section(".compile_time_request")                    \
+        = "_DECL_COMMAND " __stringify(FUNC) " " __stringify(FLAGS) " " MSG; \
+    void __visible FUNC(uint32_t*)
+
+// Create a compile time request and return a unique (incrementing id)
+// for that request.
+#define _DECL_REQUEST_ID(REQUEST, ID_SECTION) ({                \
+    static char __PASTE(_DECLS_, __LINE__)[]                    \
+        __section(".compile_time_request") = REQUEST;           \
+    asm volatile("" : : "m"(__PASTE(_DECLS_, __LINE__)));       \
+    static char __PASTE(_DECLI_, __LINE__)                      \
+        __section(".compile_time_request." ID_SECTION);         \
+    (size_t)&__PASTE(_DECLI_, __LINE__); })
+
+#define _DECL_PARSER(FMT)                               \
+    _DECL_REQUEST_ID("_DECL_PARSER " FMT, "parsers")
+
+#define _DECL_OUTPUT(FMT)                               \
+    _DECL_REQUEST_ID("_DECL_OUTPUT " FMT, "parsers")
+
+#define _DECL_STATIC_STR(FMT)                                   \
+    _DECL_REQUEST_ID("_DECL_STATIC_STR " FMT, "static_strings")
+
+#endif // command.h
diff --git a/src/compiler.h b/src/compiler.h
new file mode 100644
index 00000000..ba4b83d5
--- /dev/null
+++ b/src/compiler.h
@@ -0,0 +1,66 @@
+#ifndef __COMPILER_H
+#define __COMPILER_H
+// Low level definitions for C languange and gcc compiler.
+
+#define barrier() __asm__ __volatile__("": : :"memory")
+
+#define likely(x)       __builtin_expect(!!(x), 1)
+#define unlikely(x)     __builtin_expect(!!(x), 0)
+
+#define noinline __attribute__((noinline))
+#ifndef __always_inline
+#define __always_inline inline __attribute__((always_inline))
+#endif
+#define __visible __attribute__((externally_visible))
+#define __noreturn __attribute__((noreturn))
+
+#define PACKED __attribute__((packed))
+#define __aligned(x) __attribute__((aligned(x)))
+#define __section(S) __attribute__((section(S)))
+
+#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
+#define ALIGN(x,a)              __ALIGN_MASK(x,(typeof(x))(a)-1)
+#define __ALIGN_MASK(x,mask)    (((x)+(mask))&~(mask))
+#define ALIGN_DOWN(x,a)         ((x) & ~((typeof(x))(a)-1))
+
+#define container_of(ptr, type, member) ({                      \
+        const typeof( ((type *)0)->member ) *__mptr = (ptr);    \
+        (type *)( (char *)__mptr - offsetof(type,member) );})
+
+#define __stringify_1(x)        #x
+#define __stringify(x)          __stringify_1(x)
+
+#define ___PASTE(a,b) a##b
+#define __PASTE(a,b) ___PASTE(a,b)
+
+#define DIV_ROUND_UP(n,d) (((n) + (d) - 1) / (d))
+#define DIV_ROUND_CLOSEST(x, divisor)({                 \
+            typeof(divisor) __divisor = divisor;        \
+            (((x) + ((__divisor) / 2)) / (__divisor));  \
+        })
+
+union u32_u16_u {
+    struct { uint16_t lo, hi; };
+    uint32_t val;
+};
+
+static inline void writel(void *addr, uint32_t val) {
+    *(volatile uint32_t *)addr = val;
+}
+static inline void writew(void *addr, uint16_t val) {
+    *(volatile uint16_t *)addr = val;
+}
+static inline void writeb(void *addr, uint8_t val) {
+    *(volatile uint8_t *)addr = val;
+}
+static inline uint32_t readl(const void *addr) {
+    return *(volatile const uint32_t *)addr;
+}
+static inline uint16_t readw(const void *addr) {
+    return *(volatile const uint16_t *)addr;
+}
+static inline uint8_t readb(const void *addr) {
+    return *(volatile const uint8_t *)addr;
+}
+
+#endif // compiler.h
diff --git a/src/declfunc.lds.S b/src/declfunc.lds.S
new file mode 100644
index 00000000..9bb5c8ad
--- /dev/null
+++ b/src/declfunc.lds.S
@@ -0,0 +1,26 @@
+// Linker script that defines symbols around sections.  The DECL_X()
+// macros need this linker script to place _start and _end symbols
+// around the list of declared items.
+
+#define DECLWRAPPER(NAME)                       \
+    .progmem.data. ## NAME : SUBALIGN(1) {      \
+        NAME ## _start = . ;                    \
+        *( .progmem.data. ## NAME ##.pre* )     \
+        *( .progmem.data. ## NAME ##* )         \
+        *( .progmem.data. ## NAME ##.post* )    \
+        NAME ## _end = . ;                      \
+    }
+
+SECTIONS
+{
+    DECLWRAPPER(taskfuncs)
+    DECLWRAPPER(initfuncs)
+    DECLWRAPPER(shutdownfuncs)
+
+    .compile_time_request.static_strings 0 (INFO) : {
+        *( .compile_time_request.static_strings )
+    }
+    .compile_time_request.parsers 0 (INFO) : {
+        *( .compile_time_request.parsers )
+    }
+}
diff --git a/src/endstop.c b/src/endstop.c
new file mode 100644
index 00000000..bc177605
--- /dev/null
+++ b/src/endstop.c
@@ -0,0 +1,110 @@
+// Handling of end stops.
+//
+// Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <stddef.h> // offsetof
+#include "basecmd.h" // alloc_oid
+#include "board/gpio.h" // struct gpio
+#include "board/irq.h" // irq_save
+#include "command.h" // DECL_COMMAND
+#include "sched.h" // struct timer
+#include "stepper.h" // stepper_stop
+
+struct end_stop {
+    struct timer time;
+    uint32_t rest_time;
+    struct stepper *stepper;
+    struct gpio_in pin;
+    uint8_t pin_value, flags;
+};
+
+enum { ESF_HOMING=1, ESF_REPORT=2 };
+
+// Timer callback for an end stop
+static uint8_t
+end_stop_event(struct timer *t)
+{
+    struct end_stop *e = container_of(t, struct end_stop, time);
+    uint8_t val = gpio_in_read(e->pin);
+    if (val != e->pin_value) {
+        e->time.waketime += e->rest_time;
+        return SF_RESCHEDULE;
+    }
+    // Stop stepper
+    e->flags = ESF_REPORT;
+    stepper_stop(e->stepper);
+    return SF_DONE;
+}
+
+void
+command_config_end_stop(uint32_t *args)
+{
+    struct end_stop *e = alloc_oid(args[0], command_config_end_stop, sizeof(*e));
+    struct stepper *s = lookup_oid(args[3], command_config_stepper);
+    e->time.func = end_stop_event;
+    e->stepper = s;
+    e->pin = gpio_in_setup(args[1], args[2]);
+}
+DECL_COMMAND(command_config_end_stop,
+             "config_end_stop oid=%c pin=%c pull_up=%c stepper_oid=%c");
+
+// Home an axis
+void
+command_end_stop_home(uint32_t *args)
+{
+    struct end_stop *e = lookup_oid(args[0], command_config_end_stop);
+    sched_del_timer(&e->time);
+    e->time.waketime = args[1];
+    e->rest_time = args[2];
+    if (!e->rest_time) {
+        // Disable end stop checking
+        e->flags = 0;
+        return;
+    }
+    e->pin_value = args[3];
+    e->flags = ESF_HOMING;
+    sched_timer(&e->time);
+}
+DECL_COMMAND(command_end_stop_home,
+             "end_stop_home oid=%c clock=%u rest_ticks=%u pin_value=%c");
+
+static void
+end_stop_report(uint8_t oid, struct end_stop *e)
+{
+    uint8_t flag = irq_save();
+    uint32_t position = stepper_get_position(e->stepper);
+    uint8_t eflags = e->flags;
+    e->flags &= ~ESF_REPORT;
+    irq_restore(flag);
+
+    sendf("end_stop_state oid=%c homing=%c pin=%c pos=%i"
+          , oid, !!(eflags & ESF_HOMING), gpio_in_read(e->pin)
+          , position - STEPPER_POSITION_BIAS);
+}
+
+void
+command_end_stop_query(uint32_t *args)
+{
+    uint8_t oid = args[0];
+    struct end_stop *e = lookup_oid(oid, command_config_end_stop);
+    end_stop_report(oid, e);
+}
+DECL_COMMAND(command_end_stop_query, "end_stop_query oid=%c");
+
+static void
+end_stop_task(void)
+{
+    static uint16_t next;
+    if (!sched_check_periodic(50, &next))
+        return;
+    uint8_t oid;
+    struct end_stop *e;
+    foreach_oid(oid, e, command_config_end_stop) {
+        if (!(e->flags & ESF_REPORT))
+            continue;
+        end_stop_report(oid, e);
+    }
+}
+DECL_TASK(end_stop_task);
diff --git a/src/gpiocmds.c b/src/gpiocmds.c
new file mode 100644
index 00000000..ff03dfd1
--- /dev/null
+++ b/src/gpiocmds.c
@@ -0,0 +1,401 @@
+// Commands for controlling GPIO pins
+//
+// Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <stddef.h> // offsetof
+#include "basecmd.h" // alloc_oid
+#include "board/gpio.h" // struct gpio
+#include "board/irq.h" // irq_save
+#include "command.h" // DECL_COMMAND
+#include "sched.h" // DECL_TASK
+
+
+/****************************************************************
+ * Digital out pins
+ ****************************************************************/
+
+struct digital_out_s {
+    struct timer timer;
+    struct gpio_out pin;
+    uint32_t max_duration;
+    uint8_t value, default_value;
+};
+
+static uint8_t
+digital_end_event(struct timer *timer)
+{
+    shutdown("Missed scheduling of next pin event");
+}
+
+static uint8_t
+digital_out_event(struct timer *timer)
+{
+    struct digital_out_s *d = container_of(timer, struct digital_out_s, timer);
+    gpio_out_write(d->pin, d->value);
+    if (d->value == d->default_value || !d->max_duration)
+        return SF_DONE;
+    d->timer.waketime += d->max_duration;
+    d->timer.func = digital_end_event;
+    return SF_RESCHEDULE;
+}
+
+void
+command_config_digital_out(uint32_t *args)
+{
+    struct digital_out_s *d = alloc_oid(args[0], command_config_digital_out
+                                        , sizeof(*d));
+    d->default_value = args[2];
+    d->pin = gpio_out_setup(args[1], d->default_value);
+    d->max_duration = args[3];
+}
+DECL_COMMAND(command_config_digital_out,
+             "config_digital_out oid=%c pin=%u default_value=%c"
+             " max_duration=%u");
+
+void
+command_schedule_digital_out(uint32_t *args)
+{
+    struct digital_out_s *d = lookup_oid(args[0], command_config_digital_out);
+    sched_del_timer(&d->timer);
+    d->timer.func = digital_out_event;
+    d->timer.waketime = args[1];
+    d->value = args[2];
+    sched_timer(&d->timer);
+}
+DECL_COMMAND(command_schedule_digital_out,
+             "schedule_digital_out oid=%c clock=%u value=%c");
+
+static void
+digital_out_shutdown(void)
+{
+    uint8_t i;
+    struct digital_out_s *d;
+    foreach_oid(i, d, command_config_digital_out) {
+        gpio_out_write(d->pin, d->default_value);
+    }
+}
+DECL_SHUTDOWN(digital_out_shutdown);
+
+void
+command_set_digital_out(uint32_t *args)
+{
+    gpio_out_setup(args[0], args[1]);
+}
+DECL_COMMAND(command_set_digital_out, "set_digital_out pin=%u value=%c");
+
+
+/****************************************************************
+ * Hardware PWM pins
+ ****************************************************************/
+
+struct pwm_out_s {
+    struct timer timer;
+    struct gpio_pwm pin;
+    uint32_t max_duration;
+    uint8_t value, default_value;
+};
+
+static uint8_t
+pwm_event(struct timer *timer)
+{
+    struct pwm_out_s *p = container_of(timer, struct pwm_out_s, timer);
+    gpio_pwm_write(p->pin, p->value);
+    if (p->value == p->default_value || !p->max_duration)
+        return SF_DONE;
+    p->timer.waketime += p->max_duration;
+    p->timer.func = digital_end_event;
+    return SF_RESCHEDULE;
+}
+
+void
+command_config_pwm_out(uint32_t *args)
+{
+    struct pwm_out_s *p = alloc_oid(args[0], command_config_pwm_out, sizeof(*p));
+    p->default_value = args[3];
+    p->pin = gpio_pwm_setup(args[1], args[2], p->default_value);
+    p->max_duration = args[4];
+}
+DECL_COMMAND(command_config_pwm_out,
+             "config_pwm_out oid=%c pin=%u cycle_ticks=%u default_value=%c"
+             " max_duration=%u");
+
+void
+command_schedule_pwm_out(uint32_t *args)
+{
+    struct pwm_out_s *p = lookup_oid(args[0], command_config_pwm_out);
+    sched_del_timer(&p->timer);
+    p->timer.func = pwm_event;
+    p->timer.waketime = args[1];
+    p->value = args[2];
+    sched_timer(&p->timer);
+}
+DECL_COMMAND(command_schedule_pwm_out,
+             "schedule_pwm_out oid=%c clock=%u value=%c");
+
+static void
+pwm_shutdown(void)
+{
+    uint8_t i;
+    struct pwm_out_s *p;
+    foreach_oid(i, p, command_config_pwm_out) {
+        gpio_pwm_write(p->pin, p->default_value);
+    }
+}
+DECL_SHUTDOWN(pwm_shutdown);
+
+void
+command_set_pwm_out(uint32_t *args)
+{
+    gpio_pwm_setup(args[0], args[1], args[2]);
+}
+DECL_COMMAND(command_set_pwm_out, "set_pwm_out pin=%u cycle_ticks=%u value=%c");
+
+
+/****************************************************************
+ * Soft PWM output pins
+ ****************************************************************/
+
+struct soft_pwm_s {
+    struct timer timer;
+    uint32_t on_duration, off_duration, end_time;
+    uint32_t next_on_duration, next_off_duration;
+    uint32_t max_duration, cycle_time, pulse_time;
+    struct gpio_out pin;
+    uint8_t default_value, flags;
+};
+
+enum {
+    SPF_ON=1<<0, SPF_TOGGLING=1<<1, SPF_CHECK_END=1<<2, SPF_HAVE_NEXT=1<<3,
+    SPF_NEXT_ON=1<<4, SPF_NEXT_TOGGLING=1<<5, SPF_NEXT_CHECK_END=1<<6,
+};
+
+static uint8_t soft_pwm_load_event(struct timer *timer);
+
+// Normal pulse change event
+static uint8_t
+soft_pwm_toggle_event(struct timer *timer)
+{
+    struct soft_pwm_s *s = container_of(timer, struct soft_pwm_s, timer);
+    gpio_out_toggle(s->pin);
+    s->flags ^= SPF_ON;
+    uint32_t waketime = s->timer.waketime;
+    if (s->flags & SPF_ON)
+        waketime += s->on_duration;
+    else
+        waketime += s->off_duration;
+    if (s->flags & SPF_CHECK_END && !sched_is_before(waketime, s->end_time)) {
+        // End of normal pulsing - next event loads new pwm settings
+        s->timer.func = soft_pwm_load_event;
+        waketime = s->end_time;
+    }
+    s->timer.waketime = waketime;
+    return SF_RESCHEDULE;
+}
+
+// Load next pwm settings
+static uint8_t
+soft_pwm_load_event(struct timer *timer)
+{
+    struct soft_pwm_s *s = container_of(timer, struct soft_pwm_s, timer);
+    if (!(s->flags & SPF_HAVE_NEXT))
+        shutdown("Missed scheduling of next pwm event");
+    uint8_t flags = s->flags >> 4;
+    s->flags = flags;
+    gpio_out_write(s->pin, flags & SPF_ON);
+    if (!(flags & SPF_TOGGLING)) {
+        // Pin is in an always on (value=255) or always off (value=0) state
+        if (!(flags & SPF_CHECK_END))
+            return SF_DONE;
+        s->timer.waketime = s->end_time = s->end_time + s->max_duration;
+        return SF_RESCHEDULE;
+    }
+    // Schedule normal pin toggle timer events
+    s->timer.func = soft_pwm_toggle_event;
+    s->off_duration = s->next_off_duration;
+    s->on_duration = s->next_on_duration;
+    s->timer.waketime = s->end_time + s->on_duration;
+    s->end_time += s->max_duration;
+    return SF_RESCHEDULE;
+}
+
+void
+command_config_soft_pwm_out(uint32_t *args)
+{
+    struct soft_pwm_s *s = alloc_oid(args[0], command_config_soft_pwm_out
+                                     , sizeof(*s));
+    s->cycle_time = args[2];
+    s->pulse_time = s->cycle_time / 255;
+    s->default_value = !!args[3];
+    s->max_duration = args[4];
+    s->flags = s->default_value ? SPF_ON : 0;
+    s->pin = gpio_out_setup(args[1], s->default_value);
+}
+DECL_COMMAND(command_config_soft_pwm_out,
+             "config_soft_pwm_out oid=%c pin=%u cycle_ticks=%u default_value=%c"
+             " max_duration=%u");
+
+void
+command_schedule_soft_pwm_out(uint32_t *args)
+{
+    struct soft_pwm_s *s = lookup_oid(args[0], command_config_soft_pwm_out);
+    uint32_t time = args[1];
+    uint8_t value = args[2];
+    uint8_t next_flags = SPF_CHECK_END | SPF_HAVE_NEXT;
+    uint32_t next_on_duration, next_off_duration;
+    if (value == 0 || value == 255) {
+        next_on_duration = next_off_duration = 0;
+        next_flags |= value ? SPF_NEXT_ON : 0;
+        if (!!value != s->default_value && s->max_duration)
+            next_flags |= SPF_NEXT_CHECK_END;
+    } else {
+        next_on_duration = s->pulse_time * value;
+        next_off_duration = s->cycle_time - next_on_duration;
+        next_flags |= SPF_NEXT_ON | SPF_NEXT_TOGGLING;
+        if (s->max_duration)
+            next_flags |= SPF_NEXT_CHECK_END;
+    }
+    uint8_t flag = irq_save();
+    if (s->flags & SPF_CHECK_END && sched_is_before(s->end_time, time))
+        shutdown("next soft pwm extends existing pwm");
+    s->end_time = time;
+    s->next_on_duration = next_on_duration;
+    s->next_off_duration = next_off_duration;
+    s->flags |= next_flags;
+    if (s->flags & SPF_TOGGLING && sched_is_before(s->timer.waketime, time)) {
+        // soft_pwm_toggle_event() will schedule a load event when ready
+    } else {
+        // Schedule the loading of the pwm parameters at the requested time
+        sched_del_timer(&s->timer);
+        s->timer.waketime = time;
+        s->timer.func = soft_pwm_load_event;
+        sched_timer(&s->timer);
+    }
+    irq_restore(flag);
+}
+DECL_COMMAND(command_schedule_soft_pwm_out,
+             "schedule_soft_pwm_out oid=%c clock=%u value=%c");
+
+static void
+soft_pwm_shutdown(void)
+{
+    uint8_t i;
+    struct soft_pwm_s *s;
+    foreach_oid(i, s, command_config_soft_pwm_out) {
+        gpio_out_write(s->pin, s->default_value);
+        s->flags = s->default_value ? SPF_ON : 0;
+    }
+}
+DECL_SHUTDOWN(soft_pwm_shutdown);
+
+
+/****************************************************************
+ * Analog input pins
+ ****************************************************************/
+
+struct analog_in {
+    struct timer timer;
+    uint32_t rest_time, sample_time, next_begin_time;
+    uint16_t value, min_value, max_value;
+    struct gpio_adc pin;
+    uint8_t state, sample_count;
+};
+
+static uint8_t
+analog_in_event(struct timer *timer)
+{
+    struct analog_in *a = container_of(timer, struct analog_in, timer);
+    if (gpio_adc_sample(a->pin)) {
+        a->timer.waketime += gpio_adc_sample_time();
+        return SF_RESCHEDULE;
+    }
+    uint16_t value = gpio_adc_read(a->pin);
+    uint8_t state = a->state;
+    if (state >= a->sample_count) {
+        state = 0;
+    } else {
+        value += a->value;
+    }
+    a->value = value;
+    a->state = state+1;
+    if (a->state < a->sample_count) {
+        a->timer.waketime += a->sample_time;
+        return SF_RESCHEDULE;
+    }
+    if (a->value < a->min_value || a->value > a->max_value)
+        shutdown("adc out of range");
+    a->next_begin_time += a->rest_time;
+    a->timer.waketime = a->next_begin_time;
+    return SF_RESCHEDULE;
+}
+
+void
+command_config_analog_in(uint32_t *args)
+{
+    struct analog_in *a = alloc_oid(
+        args[0], command_config_analog_in, sizeof(*a));
+    a->timer.func = analog_in_event;
+    a->pin = gpio_adc_setup(args[1]);
+    a->state = 1;
+}
+DECL_COMMAND(command_config_analog_in, "config_analog_in oid=%c pin=%u");
+
+void
+command_query_analog_in(uint32_t *args)
+{
+    struct analog_in *a = lookup_oid(args[0], command_config_analog_in);
+    sched_del_timer(&a->timer);
+    gpio_adc_clear_sample(a->pin);
+    a->next_begin_time = args[1];
+    a->timer.waketime = a->next_begin_time;
+    a->sample_time = args[2];
+    a->sample_count = args[3];
+    a->state = a->sample_count + 1;
+    a->rest_time = args[4];
+    a->min_value = args[5];
+    a->max_value = args[6];
+    if (! a->sample_count)
+        return;
+    sched_timer(&a->timer);
+}
+DECL_COMMAND(command_query_analog_in,
+             "query_analog_in oid=%c clock=%u sample_ticks=%u sample_count=%c"
+             " rest_ticks=%u min_value=%hu max_value=%hu");
+
+static void
+analog_in_task(void)
+{
+    static uint16_t next;
+    if (!sched_check_periodic(3, &next))
+        return;
+    uint8_t oid;
+    struct analog_in *a;
+    foreach_oid(oid, a, command_config_analog_in) {
+        if (a->state != a->sample_count)
+            continue;
+        uint8_t flag = irq_save();
+        if (a->state != a->sample_count) {
+            irq_restore(flag);
+            continue;
+        }
+        uint16_t value = a->value;
+        uint32_t next_begin_time = a->next_begin_time;
+        a->state++;
+        irq_restore(flag);
+        sendf("analog_in_state oid=%c next_clock=%u value=%hu"
+              , oid, next_begin_time, value);
+    }
+}
+DECL_TASK(analog_in_task);
+
+static void
+analog_in_shutdown(void)
+{
+    uint8_t i;
+    struct analog_in *a;
+    foreach_oid(i, a, command_config_analog_in) {
+        gpio_adc_clear_sample(a->pin);
+    }
+}
+DECL_SHUTDOWN(analog_in_shutdown);
diff --git a/src/sched.c b/src/sched.c
new file mode 100644
index 00000000..2f51515c
--- /dev/null
+++ b/src/sched.c
@@ -0,0 +1,282 @@
+// Basic scheduling functions and startup/shutdown code.
+//
+// Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <setjmp.h> // setjmp
+#include <stdarg.h> // va_list
+#include <stddef.h> // NULL
+#include "autoconf.h" // CONFIG_*
+#include "board/irq.h" // irq_save
+#include "board/timer.h" // timer_from_ms
+#include "command.h" // shutdown
+#include "sched.h" // sched_from_ms
+#include "stepper.h" // stepper_event
+
+
+/****************************************************************
+ * Timers
+ ****************************************************************/
+
+static uint16_t millis;
+
+// Default millisecond timer.  This timer counts milliseconds.  It
+// also simplifies the timer code by ensuring there is always at least
+// one timer on the timer list and that there is always a timer not
+// more than 1 ms in the future.
+static uint8_t
+ms_event(struct timer *t)
+{
+    millis++;
+    timer_periodic();
+    t->waketime += sched_from_ms(1);
+    return SF_RESCHEDULE;
+}
+
+static struct timer ms_timer = {
+    .func = ms_event
+};
+
+// Check if ready for a recurring periodic event
+uint8_t
+sched_check_periodic(uint16_t time, uint16_t *pnext)
+{
+    uint16_t next = *pnext, cur;
+    uint8_t flag = irq_save();
+    cur = millis;
+    irq_restore(flag);
+    if ((int16_t)(cur - next) < 0)
+        return 0;
+    *pnext = cur + time;
+    return 1;
+}
+
+// Return the number of clock ticks for a given number of milliseconds
+uint32_t
+sched_from_ms(uint32_t ms)
+{
+    return timer_from_ms(ms);
+}
+
+// Return the current time (in clock ticks)
+uint32_t
+sched_read_time(void)
+{
+    return timer_read_time();
+}
+
+// Return true if time1 is before time2.  Always use this function to
+// compare times as regular C comparisons can fail if the counter
+// rolls over.
+uint8_t
+sched_is_before(uint32_t time1, uint32_t time2)
+{
+    return (int32_t)(time1 - time2) < 0;
+}
+
+static struct timer *timer_list = &ms_timer;
+
+// Add a timer to timer list.
+static __always_inline void
+add_timer(struct timer *add)
+{
+    struct timer **timep = &timer_list, *t = timer_list;
+    while (t && !sched_is_before(add->waketime, t->waketime)) {
+        timep = &t->next;
+        t = t->next;
+    }
+    add->next = t;
+    *timep = add;
+}
+
+// Schedule a function call at a supplied time.
+void
+sched_timer(struct timer *add)
+{
+    uint8_t flag = irq_save();
+    add_timer(add);
+
+    // Reschedule timer if necessary.
+    if (timer_list == add) {
+        uint8_t ret = timer_set_next(add->waketime);
+        if (ret)
+            shutdown("Timer too close");
+    }
+
+    irq_restore(flag);
+}
+
+// Remove a timer that may be live.
+void
+sched_del_timer(struct timer *del)
+{
+    uint8_t flag = irq_save();
+
+    if (timer_list == del) {
+        timer_list = del->next;
+        timer_set_next(timer_list->waketime);
+        irq_restore(flag);
+        return;
+    }
+
+    // Find and remove from timer list.
+    struct timer *prev = timer_list;
+    for (;;) {
+        struct timer *t = prev->next;
+        if (!t)
+            break;
+        if (t == del) {
+            prev->next = del->next;
+            break;
+        }
+        prev = t;
+    }
+
+    irq_restore(flag);
+}
+
+// Invoke timers - called from board timer irq code.
+void
+sched_timer_kick(void)
+{
+    struct timer *t = timer_list;
+    for (;;) {
+        // Invoke timer callback
+        uint8_t res;
+        if (CONFIG_INLINE_STEPPER_HACK && likely(!t->func))
+            res = stepper_event(t);
+        else
+            res = t->func(t);
+
+        // Update timer_list (rescheduling current timer if necessary)
+        timer_list = t->next;
+        if (likely(res))
+            add_timer(t);
+        t = timer_list;
+
+        // Schedule next timer event (or run next timer if it's ready)
+        res = timer_try_set_next(t->waketime);
+        if (res)
+            break;
+    }
+}
+
+// Shutdown all user timers on an emergency stop.
+static void
+timer_shutdown(void)
+{
+    timer_list = &ms_timer;
+    ms_timer.next = NULL;
+    timer_set_next(timer_list->waketime);
+}
+DECL_SHUTDOWN(timer_shutdown);
+
+
+/****************************************************************
+ * Shutdown processing
+ ****************************************************************/
+
+static uint16_t shutdown_reason;
+static uint8_t shutdown_status;
+
+// Return true if the machine is in an emergency stop state
+uint8_t
+sched_is_shutdown(void)
+{
+    return !!shutdown_status;
+}
+
+uint16_t
+sched_shutdown_reason(void)
+{
+    return shutdown_reason;
+}
+
+// Transition out of shutdown state
+void
+sched_clear_shutdown(void)
+{
+    if (!shutdown_status)
+        shutdown("Shutdown cleared when not shutdown");
+    if (shutdown_status == 2)
+        // Ignore attempt to clear shutdown if still processing shutdown
+        return;
+    shutdown_status = 0;
+}
+
+// Invoke all shutdown functions (as declared by DECL_SHUTDOWN)
+static void
+run_shutdown(void)
+{
+    shutdown_status = 2;
+    struct callback_handler *p;
+    foreachdecl(p, shutdownfuncs) {
+        void (*func)(void) = READP(p->func);
+        func();
+    }
+    shutdown_status = 1;
+    irq_enable();
+
+    sendf("shutdown static_string_id=%hu", shutdown_reason);
+}
+
+// Shutdown the machine if not already in the process of shutting down
+void
+sched_try_shutdown(uint16_t reason)
+{
+    if (shutdown_status != 2)
+        sched_shutdown(reason);
+}
+
+static jmp_buf shutdown_jmp;
+
+// Force the machine to immediately run the shutdown handlers
+void
+sched_shutdown(uint16_t reason)
+{
+    irq_disable();
+    shutdown_reason = reason;
+    longjmp(shutdown_jmp, 1);
+}
+
+
+/****************************************************************
+ * Startup and background task processing
+ ****************************************************************/
+
+// Invoke all init functions (as declared by DECL_INIT)
+static void
+run_init(void)
+{
+    struct callback_handler *p;
+    foreachdecl(p, initfuncs) {
+        void (*func)(void) = READP(p->func);
+        func();
+    }
+}
+
+// Invoke all background task functions (as declared by DECL_TASK)
+static void
+run_task(void)
+{
+    struct callback_handler *p;
+    foreachdecl(p, taskfuncs) {
+        void (*func)(void) = READP(p->func);
+        func();
+    }
+}
+
+// Main loop of program
+void
+sched_main(void)
+{
+    run_init();
+
+    int ret = setjmp(shutdown_jmp);
+    if (ret)
+        run_shutdown();
+
+    for (;;)
+        run_task();
+}
diff --git a/src/sched.h b/src/sched.h
new file mode 100644
index 00000000..047ac1ce
--- /dev/null
+++ b/src/sched.h
@@ -0,0 +1,51 @@
+#ifndef __SCHED_H
+#define __SCHED_H
+
+#include <stdint.h>
+#include "board/pgm.h" // PSTR
+#include "compiler.h" // __section
+
+// Declare an init function (called at firmware startup)
+#define DECL_INIT(FUNC) _DECL_CALLBACK(initfuncs, FUNC)
+// Declare a task function (called periodically during normal runtime)
+#define DECL_TASK(FUNC) _DECL_CALLBACK(taskfuncs, FUNC)
+// Declare a shutdown function (called on an emergency stop)
+#define DECL_SHUTDOWN(FUNC) _DECL_CALLBACK(shutdownfuncs, FUNC)
+
+// Timer structure for scheduling timed events (see sched_timer() )
+struct timer {
+    struct timer *next;
+    uint8_t (*func)(struct timer*);
+    uint32_t waketime;
+};
+
+enum { SF_DONE=0, SF_RESCHEDULE=1 };
+
+// sched.c
+uint8_t sched_check_periodic(uint16_t time, uint16_t *pnext);
+uint32_t sched_from_ms(uint32_t ms);
+uint32_t sched_read_time(void);
+uint8_t sched_is_before(uint32_t time1, uint32_t time2);
+void sched_timer(struct timer*);
+void sched_del_timer(struct timer *del);
+void sched_timer_kick(void);
+uint8_t sched_is_shutdown(void);
+uint16_t sched_shutdown_reason(void);
+void sched_clear_shutdown(void);
+void sched_try_shutdown(uint16_t reason);
+void sched_shutdown(uint16_t reason) __noreturn;
+void sched_main(void);
+
+// Compiler glue for DECL_X macros above.
+struct callback_handler {
+    void (*func)(void);
+};
+#define _DECL_CALLBACK(NAME, FUNC)                                      \
+    const struct callback_handler _DECL_ ## NAME ## _ ## FUNC __visible \
+    __section(".progmem.data." __stringify(NAME) ) = { .func = FUNC }
+
+#define foreachdecl(ITER, NAME)                                 \
+    extern typeof(*ITER) NAME ## _start[], NAME ## _end[];      \
+    for (ITER = NAME ## _start ; ITER < NAME ## _end ; ITER ++)
+
+#endif // sched.h
diff --git a/src/simulator/Kconfig b/src/simulator/Kconfig
new file mode 100644
index 00000000..539cb0aa
--- /dev/null
+++ b/src/simulator/Kconfig
@@ -0,0 +1,10 @@
+# Kconfig settings for compiling and running the firmware on the host
+# processor for simulation purposes.
+
+if MACH_SIMU
+
+config BOARD_DIRECTORY
+    string
+    default "simulator"
+
+endif
diff --git a/src/simulator/Makefile b/src/simulator/Makefile
new file mode 100644
index 00000000..0dbcdab4
--- /dev/null
+++ b/src/simulator/Makefile
@@ -0,0 +1,3 @@
+# Additional simulator build rules
+
+src-y += simulator/main.c simulator/gpio.c
diff --git a/src/simulator/gpio.c b/src/simulator/gpio.c
new file mode 100644
index 00000000..90d684e9
--- /dev/null
+++ b/src/simulator/gpio.c
@@ -0,0 +1,45 @@
+// GPIO functions on simulator.
+//
+// Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include "gpio.h" // gpio_out_write
+
+struct gpio_out gpio_out_setup(uint8_t pin, uint8_t val) {
+    return (struct gpio_out){.pin=pin};
+}
+void gpio_out_toggle(struct gpio_out g) {
+}
+void gpio_out_write(struct gpio_out g, uint8_t val) {
+}
+struct gpio_in gpio_in_setup(uint8_t pin, int8_t pull_up) {
+    return (struct gpio_in){.pin=pin};
+}
+uint8_t gpio_in_read(struct gpio_in g) {
+    return 0;
+}
+struct gpio_pwm gpio_pwm_setup(uint8_t pin, uint32_t cycle_time, uint8_t val) {
+    return (struct gpio_pwm){.pin=pin};
+}
+void gpio_pwm_write(struct gpio_pwm g, uint8_t val) {
+}
+struct gpio_adc gpio_adc_setup(uint8_t pin) {
+    return (struct gpio_adc){.pin=pin};
+}
+uint32_t gpio_adc_sample_time(void) {
+    return 0;
+}
+uint8_t gpio_adc_sample(struct gpio_adc g) {
+    return 0;
+}
+void gpio_adc_clear_sample(struct gpio_adc g) {
+}
+uint16_t gpio_adc_read(struct gpio_adc g) {
+    return 0;
+}
+
+void spi_config(void) {
+}
+void spi_transfer(char *data, uint8_t len) {
+}
diff --git a/src/simulator/gpio.h b/src/simulator/gpio.h
new file mode 100644
index 00000000..84deb7ef
--- /dev/null
+++ b/src/simulator/gpio.h
@@ -0,0 +1,37 @@
+#ifndef __SIMU_GPIO_H
+#define __SIMU_GPIO_H
+
+#include <stdint.h>
+
+struct gpio_out {
+    uint8_t pin;
+};
+struct gpio_out gpio_out_setup(uint8_t pin, uint8_t val);
+void gpio_out_toggle(struct gpio_out g);
+void gpio_out_write(struct gpio_out g, uint8_t val);
+
+struct gpio_in {
+    uint8_t pin;
+};
+struct gpio_in gpio_in_setup(uint8_t pin, int8_t pull_up);
+uint8_t gpio_in_read(struct gpio_in g);
+
+struct gpio_pwm {
+    uint8_t pin;
+};
+struct gpio_pwm gpio_pwm_setup(uint8_t pin, uint32_t cycle_time, uint8_t val);
+void gpio_pwm_write(struct gpio_pwm g, uint8_t val);
+
+struct gpio_adc {
+    uint8_t pin;
+};
+struct gpio_adc gpio_adc_setup(uint8_t pin);
+uint32_t gpio_adc_sample_time(void);
+uint8_t gpio_adc_sample(struct gpio_adc g);
+void gpio_adc_clear_sample(struct gpio_adc g);
+uint16_t gpio_adc_read(struct gpio_adc g);
+
+void spi_config(void);
+void spi_transfer(char *data, uint8_t len);
+
+#endif // gpio.h
diff --git a/src/simulator/irq.h b/src/simulator/irq.h
new file mode 100644
index 00000000..63f51290
--- /dev/null
+++ b/src/simulator/irq.h
@@ -0,0 +1,31 @@
+#ifndef __SIMU_IRQ_H
+#define __SIMU_IRQ_H
+// Definitions for irq enable/disable on host simulator
+
+#include <stdint.h>
+#include "compiler.h" // barrier
+
+extern uint8_t Interrupt_off;
+
+static inline void irq_disable(void) {
+    Interrupt_off = 1;
+    barrier();
+}
+
+static inline void irq_enable(void) {
+    barrier();
+    Interrupt_off = 0;
+}
+
+static inline uint8_t irq_save(void) {
+    uint8_t flag = Interrupt_off;
+    irq_disable();
+    return flag;
+}
+
+static inline void irq_restore(uint8_t flag) {
+    barrier();
+    Interrupt_off = flag;
+}
+
+#endif // irq.h
diff --git a/src/simulator/main.c b/src/simulator/main.c
new file mode 100644
index 00000000..2d43a138
--- /dev/null
+++ b/src/simulator/main.c
@@ -0,0 +1,102 @@
+// Main starting point for host simulator.
+//
+// Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <fcntl.h>
+#include <stdio.h>
+#include <unistd.h>
+#include "sched.h" // sched_main
+
+uint8_t Interrupt_off;
+
+
+/****************************************************************
+ * Timers
+ ****************************************************************/
+
+uint32_t
+timer_from_ms(uint32_t ms)
+{
+    return 0; // XXX
+}
+
+void
+timer_periodic(void)
+{
+}
+
+uint32_t
+timer_read_time(void)
+{
+    return 0; // XXX
+}
+
+uint8_t
+timer_set_next(uint32_t next)
+{
+    return 0;
+}
+
+uint8_t
+timer_try_set_next(uint32_t next)
+{
+    return 1;
+}
+
+
+/****************************************************************
+ * Turn stdin/stdout into serial console
+ ****************************************************************/
+
+// XXX
+char *
+console_get_input(uint8_t *plen)
+{
+    *plen = 0;
+    return NULL;
+}
+
+void
+console_pop_input(uint8_t len)
+{
+}
+
+// Return an output buffer that the caller may fill with transmit messages
+char *
+console_get_output(uint8_t len)
+{
+    return NULL;
+}
+
+// Accept the given number of bytes added to the transmit buffer
+void
+console_push_output(uint8_t len)
+{
+}
+
+
+/****************************************************************
+ * Startup
+ ****************************************************************/
+
+// Periodically sleep so we don't consume all CPU
+static void
+simu_pause(void)
+{
+    // XXX - should check that no timers are present.
+    usleep(1);
+}
+DECL_TASK(simu_pause);
+
+// Main entry point for simulator.
+int
+main(void)
+{
+    // Make stdin non-blocking
+    fcntl(STDIN_FILENO, F_SETFL, fcntl(STDIN_FILENO, F_GETFL, 0) | O_NONBLOCK);
+
+    sched_main();
+    return 0;
+}
diff --git a/src/simulator/misc.h b/src/simulator/misc.h
new file mode 100644
index 00000000..c279794e
--- /dev/null
+++ b/src/simulator/misc.h
@@ -0,0 +1,21 @@
+#ifndef __SIMU_MISC_H
+#define __SIMU_MISC_H
+
+#include <stdint.h>
+
+// main.c
+char *console_get_input(uint8_t *plen);
+void console_pop_input(uint8_t len);
+char *console_get_output(uint8_t len);
+void console_push_output(uint8_t len);
+
+static inline size_t alloc_maxsize(size_t reqsize) {
+    return reqsize;
+}
+
+#define HAVE_OPTIMIZED_CRC 0
+static inline uint16_t _crc16_ccitt(char *buf, uint8_t len) {
+    return 0;
+}
+
+#endif // misc.h
diff --git a/src/simulator/pgm.h b/src/simulator/pgm.h
new file mode 100644
index 00000000..e5f3787d
--- /dev/null
+++ b/src/simulator/pgm.h
@@ -0,0 +1,13 @@
+#ifndef __SIMU_PGM_H
+#define __SIMU_PGM_H
+// This header provides wrappers for the AVR specific "PROGMEM"
+// declarations.
+
+#define PROGMEM
+#define PSTR(S) S
+#define READP(VAR) VAR
+#define vsnprintf_P(D, S, F, A) vsnprintf(D, S, F, A)
+#define strcasecmp_P(S1, S2) strcasecmp(S1, S2)
+#define memcpy_P(DST, SRC, SIZE) memcpy((DST), (SRC), (SIZE))
+
+#endif // pgm.h
diff --git a/src/simulator/timer.h b/src/simulator/timer.h
new file mode 100644
index 00000000..f35b8278
--- /dev/null
+++ b/src/simulator/timer.h
@@ -0,0 +1,12 @@
+#ifndef __SIMU_TIMER_H
+#define __SIMU_TIMER_H
+
+#include <stdint.h>
+
+uint32_t timer_from_ms(uint32_t ms);
+void timer_periodic(void);
+uint32_t timer_read_time(void);
+uint8_t timer_set_next(uint32_t next);
+uint8_t timer_try_set_next(uint32_t next);
+
+#endif // timer.h
diff --git a/src/spicmds.c b/src/spicmds.c
new file mode 100644
index 00000000..bb37be2b
--- /dev/null
+++ b/src/spicmds.c
@@ -0,0 +1,22 @@
+// Commands for sending messages on an SPI bus
+//
+// Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include "board/gpio.h" // gpio_out_write
+#include "command.h" // DECL_COMMAND
+
+void
+command_send_spi_message(uint32_t *args)
+{
+    // For now, this only implements enough to program an ad5206 digipot
+    uint8_t len = args[1];
+    char *msg = (void*)(size_t)args[2];
+    spi_config();
+    struct gpio_out pin = gpio_out_setup(args[0], 0);
+    spi_transfer(msg, len);
+    gpio_out_write(pin, 1);
+    sendf("spi_response response=%*s", len, msg);
+}
+DECL_COMMAND(command_send_spi_message, "send_spi_message pin=%u msg=%*s");
diff --git a/src/stepper.c b/src/stepper.c
new file mode 100644
index 00000000..4679bf70
--- /dev/null
+++ b/src/stepper.c
@@ -0,0 +1,202 @@
+// Handling of stepper drivers.
+//
+// Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <stddef.h> // NULL
+#include "autoconf.h" // CONFIG_*
+#include "basecmd.h" // alloc_oid
+#include "board/gpio.h" // gpio_out_write
+#include "board/irq.h" // irq_save
+#include "command.h" // DECL_COMMAND
+#include "sched.h" // struct timer
+#include "stepper.h" // command_config_stepper
+
+
+/****************************************************************
+ * Steppers
+ ****************************************************************/
+
+struct stepper {
+    struct timer time;
+    uint32_t interval;
+    int16_t add;
+    uint16_t count;
+    struct gpio_out step_pin, dir_pin;
+    uint32_t position;
+    struct move *first, **plast;
+    uint32_t min_stop_interval;
+    // gcc (pre v6) does better optimization when uint8_t are bitfields
+    uint8_t flags : 8;
+};
+
+enum { MF_DIR=1 };
+enum { SF_LAST_DIR=1, SF_NEXT_DIR=2, SF_INVERT_STEP=4 };
+
+// Setup a stepper for the next move in its queue
+static uint8_t
+stepper_load_next(struct stepper *s)
+{
+    struct move *m = s->first;
+    if (!m) {
+        if (s->interval - s->add < s->min_stop_interval)
+            shutdown("No next step");
+        s->count = 0;
+        return SF_DONE;
+    }
+
+    s->interval = m->interval;
+    s->time.waketime += s->interval;
+    s->add = m->add;
+    s->interval += s->add;
+    s->count = m->count;
+    if (m->flags & MF_DIR) {
+        s->position = -s->position + s->count;
+        gpio_out_toggle(s->dir_pin);
+    } else {
+        s->position += s->count;
+    }
+
+    s->first = m->next;
+    move_free(m);
+    return SF_RESCHEDULE;
+}
+
+// Timer callback - step the given stepper.
+uint8_t
+stepper_event(struct timer *t)
+{
+    struct stepper *s = container_of(t, struct stepper, time);
+    gpio_out_toggle(s->step_pin);
+    uint16_t count = s->count - 1;
+    if (likely(count)) {
+        s->count = count;
+        s->time.waketime += s->interval;
+        s->interval += s->add;
+        gpio_out_toggle(s->step_pin);
+        return SF_RESCHEDULE;
+    }
+    uint8_t ret = stepper_load_next(s);
+    gpio_out_toggle(s->step_pin);
+    return ret;
+}
+
+void
+command_config_stepper(uint32_t *args)
+{
+    struct stepper *s = alloc_oid(args[0], command_config_stepper, sizeof(*s));
+    if (!CONFIG_INLINE_STEPPER_HACK)
+        s->time.func = stepper_event;
+    s->flags = args[4] ? SF_INVERT_STEP : 0;
+    s->step_pin = gpio_out_setup(args[1], s->flags & SF_INVERT_STEP ? 1 : 0);
+    s->dir_pin = gpio_out_setup(args[2], 0);
+    s->min_stop_interval = args[3];
+    s->position = STEPPER_POSITION_BIAS;
+}
+DECL_COMMAND(command_config_stepper,
+             "config_stepper oid=%c step_pin=%c dir_pin=%c"
+             " min_stop_interval=%u invert_step=%c");
+
+// Schedule a set of steps with a given timing
+void
+command_queue_step(uint32_t *args)
+{
+    struct stepper *s = lookup_oid(args[0], command_config_stepper);
+    struct move *m = move_alloc();
+    m->flags = 0;
+    if (!!(s->flags & SF_LAST_DIR) != !!(s->flags & SF_NEXT_DIR)) {
+        s->flags ^= SF_LAST_DIR;
+        m->flags |= MF_DIR;
+    }
+    m->interval = args[1];
+    m->count = args[2];
+    if (!m->count)
+        shutdown("Invalid count parameter");
+    m->add = args[3];
+    m->next = NULL;
+
+    uint8_t flag = irq_save();
+    if (s->count) {
+        if (s->first)
+            *s->plast = m;
+        else
+            s->first = m;
+        s->plast = &m->next;
+    } else {
+        s->first = m;
+        stepper_load_next(s);
+        sched_timer(&s->time);
+    }
+    irq_restore(flag);
+}
+DECL_COMMAND(command_queue_step,
+             "queue_step oid=%c interval=%u count=%hu add=%hi");
+
+// Set the direction of the next queued step
+void
+command_set_next_step_dir(uint32_t *args)
+{
+    struct stepper *s = lookup_oid(args[0], command_config_stepper);
+    s->flags = (s->flags & ~SF_NEXT_DIR) | (args[1] ? SF_NEXT_DIR : 0);
+}
+DECL_COMMAND(command_set_next_step_dir, "set_next_step_dir oid=%c dir=%c");
+
+// Set an absolute time that the next step will be relative to
+void
+command_reset_step_clock(uint32_t *args)
+{
+    struct stepper *s = lookup_oid(args[0], command_config_stepper);
+    uint32_t waketime = args[1];
+    if (s->count)
+        shutdown("Can't reset time when stepper active");
+    s->time.waketime = waketime;
+}
+DECL_COMMAND(command_reset_step_clock, "reset_step_clock oid=%c clock=%u");
+
+// Return the current stepper position.  Caller must disable irqs.
+uint32_t
+stepper_get_position(struct stepper *s)
+{
+    uint32_t position = s->position - s->count;
+    if (position & 0x80000000)
+        return -position;
+    return position;
+}
+
+// Reset the internal state of a 'struct stepper'
+static void
+stepper_reset(struct stepper *s)
+{
+    s->position = stepper_get_position(s);
+    s->count = 0;
+    s->flags &= SF_INVERT_STEP;
+    gpio_out_write(s->dir_pin, 0);
+}
+
+// Stop all moves for a given stepper (used in end stop homing).  IRQs
+// must be off.
+void
+stepper_stop(struct stepper *s)
+{
+    sched_del_timer(&s->time);
+    stepper_reset(s);
+    while (s->first) {
+        struct move *next = s->first->next;
+        move_free(s->first);
+        s->first = next;
+    }
+}
+
+static void
+stepper_shutdown(void)
+{
+    uint8_t i;
+    struct stepper *s;
+    foreach_oid(i, s, command_config_stepper) {
+        stepper_reset(s);
+        s->first = NULL;
+        gpio_out_write(s->step_pin, s->flags & SF_INVERT_STEP ? 1 : 0);
+    }
+}
+DECL_SHUTDOWN(stepper_shutdown);
diff --git a/src/stepper.h b/src/stepper.h
new file mode 100644
index 00000000..9a81b56e
--- /dev/null
+++ b/src/stepper.h
@@ -0,0 +1,14 @@
+#ifndef __STEPPER_H
+#define __STEPPER_H
+
+#include <stdint.h> // uint8_t
+
+enum { STEPPER_POSITION_BIAS=0x40000000 };
+
+uint8_t stepper_event(struct timer *t);
+void command_config_stepper(uint32_t *args);
+struct stepper;
+uint32_t stepper_get_position(struct stepper *s);
+void stepper_stop(struct stepper *s);
+
+#endif // stepper.h