Moved action to top level.

Removed old iterations of the project and moved the files from 02-usart
to the root directory since that's the sole place where the action is
and that subproject has outgrown its initial title.

15 files changed, 1625 insertions, 0 deletions

diff --git a/src/arch/stm32l4xxx/peripherals/clock.c b/src/arch/stm32l4xxx/peripherals/clock.c
new file mode 100644
index 0000000..9051572
--- /dev/null
+++ b/src/arch/stm32l4xxx/peripherals/clock.c
@@ -0,0 +1,117 @@
+/*
+ * This file sets the system clock to its full glory of 80Mhz
+ */
+
+#include "arch/stm32l4xxx/peripherals/clock.h"
+#include "arch/stm32l4xxx/peripherals/flash.h"
+
+#include "kern/init.h"
+
+#include <stdint.h>
+
+#define TIMEOUT 10000
+
+
+int pll_off()
+{
+  uint32_t c;
+
+  RCC.c_r &= ~BIT(24); /* Turn off pll. */
+  for (c = 0; c < TIMEOUT && RCC.c_r & BIT(25); ++c)
+    ; /* Wait for OFF. */
+
+  if (c == TIMEOUT) {
+    return E_TIMEOUT;
+  }
+
+  return 0;
+}
+
+int pll_on()
+{
+  uint32_t c;
+
+  RCC.c_r |= BIT(24); /* Turn on PLL. */
+  for (c = 0; c < TIMEOUT && !(RCC.c_r & BIT(25)); ++c)
+    ; /* Wait for RDY. */
+
+  if (c == TIMEOUT) {
+    return E_TIMEOUT;
+  }
+
+  return 0;
+}
+
+int configure_pll(
+    uint8_t pllp_div_factor, pll_divisor_t pllr, /* System clock divisor. */
+    pll_divisor_t pllq,  /* Divison factor for PLL48M1CLK. */
+    pllp_divisor_t pllp, /* Divison factor for PLLSAI2CLK. */
+    uint8_t plln,        /* PLL numerator. */
+    pllm_divisor_t pllm, /* PLL denominator. */
+    pll_src_t pllsrc /* PLL source */)
+{
+  if (RCC.c_r & BIT(25)) {
+    /* PLL must be off to configure it. */
+    return E_NOT_OFF;
+  }
+
+  /* Make sure inputs are valid. */
+  if (pllp_div_factor == 1 || pllp_div_factor > 31) {
+    return E_BADPLLP_DIV;
+  }
+  if (plln < 8 || plln > 86) {
+    return E_BADPLLN;
+  }
+
+  RCC.pllcfg_r = (pllp_div_factor << 27) | (pllr << 24) | (pllq << 20) |
+                 (pllp << 16) | (plln << 8) | (pllm << 4) | (pllsrc << 0);
+
+  return 0;
+}
+
+static _no_init uint8_t clock_mHz;
+uint8_t get_clock_mhz()
+{
+  return clock_mHz;
+}
+
+int set_system_clock_MHz(uint8_t mhz)
+{
+  clock_mHz = mhz;
+
+  /* Set the source of the system colck to MSI temporarily. */
+  set_system_clock_src(SYSTEM_CLOCK_SRC_MSI);
+
+  if (mhz <= 8 || mhz > 80) {
+    return E_BAD_ARG;
+  }
+
+  pll_off();
+
+  configure_pll(
+      0 /* pllp_div_factor */, PLL_DIVISOR_4 /* pllr: VCO / 4 = mhz MHz. */,
+      PLL_DIVISOR_4 /* pllq: VCO / 4 = mhz MHz */, PLLP_DIVISOR_7 /* pllp */,
+
+      /* The following set the frequency of VCO to (mhz*4)MHz: mhz * 1 * 4MHz.
+       */
+      mhz /* plln    | mhz */, PLLM_DIVISOR_1 /* pllm    | 01 */,
+      PLL_SRC_MSI /* pll src | 04 Mhz */);
+
+  pll_on();
+
+  /* Configure the flash to have 4 wait states. This is required at
+   * 80 MHz. */
+  FLASH.ac_r &= ~0x07;
+  FLASH.ac_r |= 0x04;
+
+  /* Set the source of the system colck to PLL. */
+  set_system_clock_src(SYSTEM_CLOCK_SRC_PLL);
+  return 0;
+}
+
+int set_system_clock_src(system_clock_src_t src)
+{
+  uint8_t value = RCC.cfg_r & ~0x03;
+  RCC.cfg_r = value | src;
+  return 0;
+}
diff --git a/src/arch/stm32l4xxx/peripherals/irq.c b/src/arch/stm32l4xxx/peripherals/irq.c
new file mode 100644
index 0000000..364b9a7
--- /dev/null
+++ b/src/arch/stm32l4xxx/peripherals/irq.c
@@ -0,0 +1,96 @@
+#include "arch/stm32l4xxx/peripherals/irq.h"
+#include "arch/stm32l4xxx/peripherals/gpio.h"
+#include "arch/stm32l4xxx/peripherals/nvic.h"
+
+#include "arch.h"
+#include "kern/delay.h"
+#include "kern/gpio/gpio_manager.h"
+
+#define IRQ_RESERVED(n)
+#define IRQ(name, uname_, n) \
+  void WEAK name () { \
+    unhandled_isr(n); \
+  }
+#include "arch/stm32l4xxx/peripherals/isrs.inc"
+#undef IRQ_RESERVED
+#undef IRQ
+
+void isr_simple_pin_on()
+{
+  int ec;
+  gpio_pin_opts_t opts = DEFAULT_GPIO_OPTS_OUTPUT;
+  gpio_reserved_pin_t pin3 = reserve_gpio_pin(GPIO_PIN_PB3, &opts, &ec);
+
+  set_gpio_pin_high(pin3);
+}
+
+#define IRQ_RESERVED(n) 0,
+#define IRQ(name, uname_, n) name,
+const void* vectors[] __attribute__((section(".vectors"))) = {
+    (void*)0x2000c000, /* Top of stack at top of sram1. 48k */
+#include "arch/stm32l4xxx/peripherals/isrs.inc"
+};
+#undef IRQ_RESERVED
+#undef IRQ
+
+/* Encodes the provided number as a series of flashes on the on-board
+ * LED. The flashes follow as such:
+ *
+ * Before the bits of the code are flashed, a rapid succession of 20 flashes
+ * followed by a pause will occur indicating that the next 8 flashes indicate
+ * the bits of the provided code.
+ *
+ * Eoch of the next eight flashes indicate either a 1 or 0 depending on the
+ * length of flash. The first flash is the least-significant bit, the next the
+ * second least, the third third least, etc.
+ *
+ * - A quick flash followed by a long pause indicates a 0 bit.
+ * - A "long" flash followed by a equally long pause indicates a 1 bit.
+ */
+void unhandled_isr(uint8_t number)
+{
+  int ec;
+  gpio_pin_opts_t opts = DEFAULT_GPIO_OPTS_OUTPUT;
+  gpio_reserved_pin_t pin3 = reserve_gpio_pin(GPIO_PIN_PB3, &opts, &ec);
+
+  for (;;) {
+    for (int i = 0; i < 20; ++ i) {
+       set_gpio_pin_high(pin3);
+       delay(1000000);
+       set_gpio_pin_low(pin3);
+       delay(1000000);
+    }
+    delay(50000000);
+
+    int n = number;
+    for (int i = 0; i < 8; ++ i) {
+      if (n & 1) {
+       // LSB is a 1
+       set_gpio_pin_high(pin3);
+       delay(15000000);
+       set_gpio_pin_low(pin3);
+       delay(15000000);
+      } else {
+       // LSB is a 0
+       set_gpio_pin_high(pin3);
+       delay(1000000);
+       set_gpio_pin_low(pin3);
+       delay(29000000);
+      }
+
+      n >>= 1;
+    }
+  }
+}
+
+void enable_interrupts(interrupt_set_t* interrupts)
+{
+  for (int i = 0; i < sizeof(NVIC.ise_r) / sizeof(uint32_t); ++ i)
+    NVIC.ise_r[i] = interrupts->irqs[i];
+}
+
+void disable_interrupts(interrupt_set_t* interrupts)
+{
+  for (int i = 0; i < sizeof(NVIC.ise_r) / sizeof(uint32_t); ++ i)
+    NVIC.ice_r[i] = interrupts->irqs[i];
+}
diff --git a/src/arch/stm32l4xxx/peripherals/usart.c b/src/arch/stm32l4xxx/peripherals/usart.c
new file mode 100644
index 0000000..7309b48
--- /dev/null
+++ b/src/arch/stm32l4xxx/peripherals/usart.c
@@ -0,0 +1,152 @@
+#include "arch/stm32l4xxx/peripherals/usart.h"
+#include "kern/delay.h"
+#include "kern/lib.h"
+#include <stdarg.h>
+
+void set_usart1_clock_src(__IO rcc_t* rcc, usart_clk_src_t usart_clk_src)
+{
+  rcc->ccip_r = (rcc->ccip_r & (~0x03)) | usart_clk_src;
+}
+
+void set_usart2_clock_src(__IO rcc_t* rcc, usart_clk_src_t usart_clk_src)
+{
+  rcc->ccip_r = (rcc->ccip_r & ~(0x03 << 2)) | (usart_clk_src << 2);
+}
+
+void set_usart2_clock_enabled(__IO rcc_t* rcc, bool enable)
+{
+  if (enable) {
+    rcc->apb1en1_r |= BIT(17);
+  } else {
+    rcc->apb1en1_r &= ~BIT(17);
+  }
+}
+
+void set_usart1_clock_enabled(__IO rcc_t* rcc, bool enable)
+{
+  if (enable) {
+    rcc->apb2en_r |= BIT(14);
+  } else {
+    rcc->apb2en_r &= ~BIT(14);
+  }
+}
+
+void usart_set_parity(__IO usart_t* usart, usart_parity_t parity)
+{
+  uint32_t c_r1 = usart->c_r1;
+  c_r1 &= ~(0x3 << 9);
+  c_r1 |= parity;
+  usart->c_r1 = c_r1;
+}
+
+void usart_set_enabled(__IO usart_t* usart, usart_enable_t enabled)
+{
+  if (!enabled) {
+    regset(usart->c_r1, usart_ue, 0);
+  } else {
+    /* Set the rx enabled. */
+    regset(usart->c_r1, usart_re, !!(enabled & USART_ENABLE_RX));
+    regset(usart->c_r1, usart_te, !!(enabled & USART_ENABLE_TX));
+    regset(usart->c_r1, usart_ue, 1);
+  }
+}
+
+void usart_transmit_byte_sync(__IO usart_t* usart, uint8_t byte)
+{
+  usart->td_r = byte;
+  /* Per the manual, when bit 7 of the IS register is set, then the usart
+   * data has been sent to the shift register.
+   *
+   * This bit is cleared by writing to the TD register. */
+  while (!(usart->is_r & BIT(7)))
+    ;
+}
+
+void usart_transmit_bytes_sync(__IO usart_t* usart, const uint8_t* bytes, uint32_t n)
+{
+  while (n --) {
+    usart_transmit_byte_sync(usart, *(bytes ++));
+  }
+}
+
+void usart_transmit_str_sync(__IO usart_t* usart, const char* str)
+{
+  while (*str) {
+    if (*str == '\n') {
+      usart_transmit_byte_sync(usart, '\r');
+    }
+    usart_transmit_byte_sync(usart, *(str ++));
+  }
+}
+
+void usart_enable_dma(__IO usart_t* usart, usart_enable_t enabled)
+{
+  switch(enabled) {
+    case USART_ENABLE_DISABLED:
+      regset(usart->c_r3, usart_dmar, 0);
+      regset(usart->c_r3, usart_dmat, 0);
+      break;
+
+    case USART_ENABLE_TX:
+      regset(usart->c_r3, usart_dmat, 1);
+      break;
+
+    case USART_ENABLE_RX:
+      regset(usart->c_r3, usart_dmar, 1);
+      break;
+  };
+}
+
+void usart_vprintf(__IO usart_t* usart, const char* fmt, va_list l)
+{
+  union {
+    void* ptr;
+    char* str;
+    int i;
+  } b;
+  char buf[128];
+
+  while (*fmt != 0) {
+    if (*fmt == '%') {
+      switch (*(++fmt)) {
+        case 0:
+          goto end;
+        case '%':
+          usart_transmit_byte_sync(usart, '%');
+          break;
+        case 'p':
+          b.ptr = va_arg(l, void*);
+          hexify(ptr2reg(b.ptr), buf);
+          usart_transmit_str_sync(usart, "0x");
+          usart_transmit_str_sync(usart, buf);
+          break;
+        case 'd':
+        case 'i':
+          b.i = va_arg(l, int);
+          decimalify(b.i, buf);
+          usart_transmit_str_sync(usart, buf);
+          break;
+        case 's':
+          b.str = va_arg(l, char*);
+          usart_transmit_str_sync(usart, b.str);
+      }
+      ++ fmt;
+    } else {
+      if (*fmt == '\n') {
+        usart_transmit_byte_sync(usart, '\r');
+      }
+      usart_transmit_byte_sync(usart, *(fmt ++));
+    }
+  }
+
+end:
+  va_end(l);
+}
+
+void usart_printf(__IO usart_t* usart, const char* fmt, ...)
+{
+  va_list l;
+  va_start(l, fmt);
+
+  usart_vprintf(usart, fmt, l);
+}
diff --git a/src/kern/delay.c b/src/kern/delay.c
new file mode 100644
index 0000000..28ef710
--- /dev/null
+++ b/src/kern/delay.c
@@ -0,0 +1,9 @@
+#include "kern/delay.h"
+
+void delay(uint32_t delay)
+{
+  while (delay--) {
+    /* needed to keep the compiler from optimizing away the loop. */
+    asm volatile("");
+  }
+}
diff --git a/src/kern/dma/dma_manager.c b/src/kern/dma/dma_manager.c
new file mode 100644
index 0000000..00e9f3d
--- /dev/null
+++ b/src/kern/dma/dma_manager.c
@@ -0,0 +1,311 @@
+#include "kern/dma/dma_manager.h"
+#include "arch/stm32l4xxx/peripherals/dma.h"
+#include "arch/stm32l4xxx/peripherals/usart.h"
+#include "arch/stm32l4xxx/peripherals/rcc.h"
+
+
+/* Bitmask of DMA2 channels in use. */
+static uint8_t dma_inuse[2];
+
+static inline dma_t* get_dma(int dma)
+{
+  if (dma) {
+    return &DMA2;
+  } else {
+    return &DMA1;
+  }
+}
+
+static dma_t* get_raw_dma(dma_channel_t chan)
+{
+  return get_dma(chan.dma);
+}
+
+static dma_channel_config_t* get_raw_channel_config(dma_channel_t chan)
+{
+  dma_t* dma = get_raw_dma(chan);
+  return &dma->channel_config[chan.chan];
+}
+
+static uint32_t get_periph_location(dma_peripheral_t operipheral)
+{
+#define CASE(p, n) case p: return ptr2reg(n);
+  switch (operipheral) {
+    CASE(DMA1_PERIPH_USART1_RX, &USART1.rd_r)
+    CASE(DMA1_PERIPH_USART1_TX, &USART1.td_r)
+    CASE(DMA1_PERIPH_USART2_RX, &USART2.rd_r)
+    CASE(DMA1_PERIPH_USART2_TX, &USART2.td_r)
+
+    default:
+      return 0;
+  };
+#undef CASE
+}
+
+static dma_channel_t allocate_dma_channel(
+    dma_peripheral_t operipheral, int* modesel)
+{
+  dma_peripheral_t peripheral = operipheral & 0xff;
+  int dmasel = peripheral >= DMA2_DMA1_SWITCH__;
+  if (dmasel) {
+    peripheral -= DMA2_DMA1_SWITCH__;
+  }
+  int chan = peripheral % DMA_N_CHANNELS;
+
+  *modesel = peripheral / 7;
+  return (dma_channel_t) {
+    .dma = dmasel,
+    .chan = chan
+  };
+}
+
+/*
+ * Atomically reserves the DMA channel so other calls
+ * cannot erroneously reserve the same DMA channel.
+ *
+ * Returns 0 if this function was unable to reserve
+ * the channel.
+ */
+static int try_reserve_dma_channel(
+    dma_channel_t chan)
+{
+  int in_use = __sync_fetch_and_or(
+      &dma_inuse[chan.dma], 1 << chan.chan);
+
+  return !(in_use & (1 << chan.chan));
+}
+
+void release_dma_channel(dma_channel_t chan)
+{
+  dma_channel_config_t* config = get_raw_channel_config(chan);
+  regset(config->cc_r, dma_cc_en, 0);       /* Disable the register. */
+  dma_inuse[chan.dma] &= ~(1 << chan.chan); /* Release the DMA. */
+
+  if (!dma_inuse[chan.dma]) {
+    /* Power-down the DMA if not in use. */
+    if (chan.dma) {
+      regset(RCC.ahb1en_r, rcc_dma2en, 0);
+    } else {
+      regset(RCC.ahb1en_r, rcc_dma1en, 0);
+    }
+  }
+}
+
+void configure_dma_channel(
+    dma_channel_t chan,
+    dma_peripheral_t operipheral,
+    dma_opts_t* opts,
+    dma_dir_t dir,
+    int selmode,
+    bool mem2mem,
+    int* error_out)
+{
+  if (chan.dma) {
+    regset(RCC.ahb1en_r, rcc_dma2en, 1);
+  } else {
+    regset(RCC.ahb1en_r, rcc_dma1en, 1);
+  }
+
+  dma_t* dma = get_raw_dma(chan);
+  regset(dma->csel_r, 0xF << (4 * chan.chan), selmode);
+  dma_channel_config_t* config = 
+    &dma->channel_config[chan.chan];
+
+  uint32_t reg = 0;
+
+  regset(reg, dma_cc_dir,     dir);
+  regset(reg, dma_cc_tcie,    opts->transfer_complete_interrupt_enable);
+  regset(reg, dma_cc_htie,    opts->half_transfer_interrupt_enable);
+  regset(reg, dma_cc_teie,    opts->transfer_error_interrupt_enable);
+  regset(reg, dma_cc_circ,    opts->circular_mode);
+  regset(reg, dma_cc_pinc,    opts->peripheral_increment);
+  regset(reg, dma_cc_minc,    opts->memory_increment);
+  regset(reg, dma_cc_psize,   opts->peripheral_block_size);
+  regset(reg, dma_cc_msize,   opts->memory_block_size);
+  regset(reg, dma_cc_pl,      opts->priority);
+  regset(reg, dma_cc_mem2mem, mem2mem);
+
+  config->cc_r = reg;
+  config->cpa_r = get_periph_location(operipheral);
+
+  *error_out = 0;
+}
+
+dma_mem2mem_channel_t select_dma_channel_mem2mem(
+    int channel,
+    dma_opts_t* opts,
+    int* error_out)
+{
+
+#define WRAP(c) ((dma_mem2mem_channel_t) { .c_ = c })
+  // TODO this should probably be in a critical section.
+  dma_channel_t chan;
+  if (channel == -1) {
+    chan.dma = 1;
+    if ((dma_inuse[chan.dma] & 0x7F) == 0x7F) {
+      chan.dma = 0;
+    }
+
+    if ((dma_inuse[chan.dma] & 0x7F) == 0x7F) {
+      *error_out = DMA_ERROR_CHANNEL_IN_USE;
+      return WRAP(DMA_CHAN_ERROR);
+    }
+
+    uint8_t t = ~(dma_inuse[chan.dma] << 1);
+    chan.chan = 6 - (__builtin_clz(t) - 24);
+  } else {
+    if (channel < 7) {
+      chan.dma = 0;
+      chan.chan = channel;
+    } else {
+      chan.dma = 0;
+      chan.chan = channel - 7;
+    }
+  }
+
+  if (!try_reserve_dma_channel(chan)) {
+    *error_out = DMA_ERROR_CHANNEL_IN_USE;
+    return WRAP(DMA_CHAN_ERROR);
+  }
+
+  int ec = 0;
+  configure_dma_channel(
+      chan,
+      -1 /* No peripheral */,
+      opts,
+      READ_FROM_PERIPHERAL,
+      /* selmode = */ 0x8,
+      /* mem2mem = */ true,
+      &ec);
+
+  if (ec) {
+    *error_out = ec;
+    return WRAP(DMA_CHAN_ERROR);
+  }
+
+  *error_out = 0;
+  return WRAP(chan);
+#undef WRAP
+}
+
+dma_mem2p_channel_t select_dma_channel_mem2p(
+    dma_peripheral_t peripheral,
+    dma_opts_t* opts_in,
+    int* error_out)
+{
+#define WRAP(c) ((dma_mem2p_channel_t) { .c_ = c })
+  *error_out = 0;
+
+  int modesel;
+  dma_channel_t ret =
+    allocate_dma_channel(peripheral, &modesel);
+
+  if (!try_reserve_dma_channel(ret)) {
+    *error_out = DMA_ERROR_CHANNEL_IN_USE;
+    return WRAP(DMA_CHAN_ERROR);
+  }
+
+  configure_dma_channel(
+      ret,
+      peripheral,
+      opts_in,
+      READ_FROM_MEMORY,
+      modesel,
+      /* mem2mem = */ false,
+      error_out);
+
+  if (*error_out) {
+    return WRAP(DMA_CHAN_ERROR);
+  }
+  
+  *error_out = 0;
+  return WRAP(ret);
+#undef WRAP
+}
+
+dma_p2mem_channel_t select_dma_channel_p2mem(
+    dma_peripheral_t peripheral,
+    dma_opts_t* opts_in,
+    int* error_out)
+{
+#define WRAP(c) ((dma_p2mem_channel_t) { .c_ = c })
+  *error_out = 0;
+
+  int modesel;
+  dma_channel_t ret =
+    allocate_dma_channel(peripheral, &modesel);
+
+  if (!try_reserve_dma_channel(ret)) {
+    *error_out = DMA_ERROR_CHANNEL_IN_USE;
+    return WRAP(DMA_CHAN_ERROR);
+  }
+
+  configure_dma_channel(
+      ret,
+      peripheral,
+      opts_in,
+      READ_FROM_PERIPHERAL,
+      modesel,
+      /* mem2mem = */ false,
+      error_out);
+
+  if (*error_out) {
+    return WRAP(DMA_CHAN_ERROR);
+  }
+  
+  *error_out = 0;
+  return WRAP(ret);
+#undef WRAP
+}
+
+
+void dma_mem2p_initiate_transfer(
+    dma_mem2p_channel_t chan, const void* from_loc, uint16_t nblocks)
+{
+  dma_channel_config_t* config = get_raw_channel_config(chan.c_);
+  config->cma_r = ptr2reg(from_loc);
+  config->cndt_r = nblocks;
+
+  regset(config->cc_r, dma_cc_en, 1);
+}
+
+void dma_mem2mem_initiate_transfer(
+    dma_mem2mem_channel_t chan,
+    void* to_loc,
+    const void* from_loc,
+    uint16_t nblocks)
+{
+  dma_channel_config_t* config = get_raw_channel_config(chan.c_);
+  config->cma_r = ptr2reg(to_loc);
+  config->cpa_r = ptr2reg(from_loc);
+  config->cndt_r = nblocks;
+
+  regset(config->cc_r, dma_cc_en, 1);
+}
+
+void dma_p2mem_initiate_transfer(
+    dma_p2mem_channel_t chan, void* to_loc, uint16_t nblocks)
+{
+  dma_channel_config_t* config = get_raw_channel_config(chan.c_);
+
+  config->cma_r = ptr2reg(to_loc);
+  config->cndt_r = nblocks;
+
+  regset(config->cc_r, dma_cc_en, 1);
+}
+
+interrupt_t dma_channel_get_interrupt(dma_channel_t chan)
+{
+  if (chan.dma == 0) {
+    return IRQ_DMA1_CHANNEL1_IRQ + chan.chan;
+  } else {
+    switch (chan.chan) {
+    case 5:
+      return IRQ_DMA1_CHANNEL6_IRQ;
+    case 6:
+      return IRQ_DMA1_CHANNEL7_IRQ;
+    default:
+      return IRQ_DMA2_CHANNEL1_IRQ + chan.chan;
+    }
+  }
+}
diff --git a/src/kern/gpio/gpio_manager.c b/src/kern/gpio/gpio_manager.c
new file mode 100644
index 0000000..82dd0ba
--- /dev/null
+++ b/src/kern/gpio/gpio_manager.c
@@ -0,0 +1,402 @@
+#include "kern/gpio/gpio_manager.h"
+
+#include "arch/stm32l4xxx/peripherals/irq.h"
+#include "arch/stm32l4xxx/peripherals/rcc.h"
+
+/* A list of whether the pins are in use or not as a bitmask. */
+uint32_t pins_inuse[N_GPIO_PINS / 32 + (N_GPIO_PINS % 32 != 0)];
+
+struct gpio_afn_and_pin {
+  int8_t afn_number;
+  gpio_pin_t gpio_pin;
+};
+
+/*
+ * Returns which (pin, afn) pairs provide the given alternate function.
+ * The out array needs to have 5 positions.
+ *
+ * This function will use afn_number = -1 as the terminal.
+ *
+ * Note that EVENTOUT is a special case because all pins have an event out 
+ * at afn=15 and should be assumed by other logic and thus is not handled
+ * by this function.
+ */
+static void get_ports_and_pins_for_alternate_function(
+    gpio_alternate_function_t afn, struct gpio_afn_and_pin* out)
+{
+  switch (afn) {
+#define AFN1(fn, ...) \
+    static_assert(false, "Unable to parse afn_table at " #fn);
+#define AFN3(fn, ...) \
+    static_assert(false, "Unable to parse afn_table at " #fn);
+#define AFN5(fn, ...) \
+    static_assert(false, "Unable to parse afn_table at " #fn);
+#define AFN7(fn, ...) \
+    static_assert(false, "Unable to parse afn_table at " #fn);
+
+#define AFN2(fn, afn, pin) \
+    out[0].afn_number = afn; \
+    out[0].gpio_pin = GPIO_PIN_ ## pin
+
+#define AFN4(fn, afn0, pin0, afn1, pin1) \
+    AFN2(fn, afn0, pin0); \
+    out[1].afn_number = afn1; \
+    out[1].gpio_pin = GPIO_PIN_ ## pin1
+
+#define AFN6(fn, afn0, pin0, afn1, pin1, afn2, pin2) \
+    AFN4(fn, afn0, pin0, afn1, pin1); \
+    out[2].afn_number = afn2; \
+    out[2].gpio_pin = GPIO_PIN_ ## pin2
+
+#define AFN8(fn, afn0, pin0, afn1, pin1, afn2, pin2, afn3, pin3) \
+    AFN6(fn, afn0, pin0, afn1, pin1, afn2, pin2); \
+    out[2].afn_number = afn3; \
+    out[2].gpio_pin = GPIO_PIN_ ## pin3
+
+#define GET_MACRO(_1, _2, _3, _4, _5, _6, _7, _8, NAME, ...) NAME
+#define GET_N(_1, _2, _3, _4, _5, _6, _7, _8, NAME, ...) NAME
+#define AFN(fn, ...) \
+    case GPIO_ALTERNATE_FUNCTION_ ## fn: \
+    GET_MACRO(__VA_ARGS__, AFN8, AFN7, AFN6, AFN5, AFN4, AFN3, AFN2, AFN1)\
+    (fn, __VA_ARGS__); \
+    out[GET_N(__VA_ARGS__, 4, 4, 3, 3, 2, 2, 1, 1)] = \
+      (struct gpio_afn_and_pin){-1, -1}; \
+    break;
+
+#include "arch/stm32l4xxx/peripherals/tables/stm32l432xx/gpio/afn_table.inc"
+    case GPIO_ALTERNATE_FUNCTION_EVENTOUT:
+      return;
+  }
+}
+
+static inline int offset_for_gpio_pin(gpio_pin_t pin)
+{
+  switch (pin) {
+#define PORT(p, pn) \
+    case GPIO_PIN_P ## p ## pn: return pn;
+#include "arch/stm32l4xxx/peripherals/tables/stm32l432xx/gpio/port_table.inc"
+#undef PORT
+    case N_GPIO_PINS: return -1;
+  }
+
+  /* Should be unreachable. */
+  return -1;
+}
+
+inline bool gpio_pin_in_use(gpio_pin_t pin)
+{
+  return !!(pins_inuse[pin / 32] & (1 << (pin % 32)));
+}
+
+#define A(...)
+#define B(...)
+#define C(...)
+#define D(...)
+#define E(...)
+#define F(...)
+#define G(...)
+#define H(...)
+#define I(...)
+#define SELECT_MACRO(PORT) PORT
+#define PORT(port, pin) \
+  SELECT_MACRO(port)(GPIO_PIN_P ## port ## pin, pin)
+static int gc_port_a()
+{
+  return 0
+#undef A
+#define A(abspin, relpin) \
+  | (gpio_pin_in_use(abspin) << (relpin))
+#include "arch/stm32l4xxx/peripherals/tables/stm32l432xx/gpio/port_table.inc"
+    ;
+#undef A
+#define A(...)
+}
+
+static int gc_port_b()
+{
+  return 0
+#undef B
+#define B(abspin, relpin) \
+  | (gpio_pin_in_use(abspin) << (relpin))
+#include "arch/stm32l4xxx/peripherals/tables/stm32l432xx/gpio/port_table.inc"
+    ;
+#undef B
+#define B(...)
+}
+
+static int gc_port_c()
+{
+  return 0
+#undef C
+#define C(abspin, relpin) \
+  | (gpio_pin_in_use(abspin) << (relpin))
+#include "arch/stm32l4xxx/peripherals/tables/stm32l432xx/gpio/port_table.inc"
+    ;
+#undef C
+#define C(...)
+}
+
+static int gc_port_d()
+{
+  return 0
+#undef D
+#define D(abspin, relpin) \
+  | (gpio_pin_in_use(abspin) << (relpin))
+#include "arch/stm32l4xxx/peripherals/tables/stm32l432xx/gpio/port_table.inc"
+    ;
+#undef D
+#define D(...)
+}
+
+static int gc_port_e()
+{
+  return 0
+#undef E
+#define E(abspin, relpin) \
+  | (gpio_pin_in_use(abspin) << (relpin))
+#include "arch/stm32l4xxx/peripherals/tables/stm32l432xx/gpio/port_table.inc"
+    ;
+#undef E
+#define E(...)
+}
+
+static int gc_port_f()
+{
+  return 0
+#undef F
+#define F(abspin, relpin) \
+  | (gpio_pin_in_use(abspin) << (relpin))
+#include "arch/stm32l4xxx/peripherals/tables/stm32l432xx/gpio/port_table.inc"
+    ;
+#undef F
+#define F(...)
+}
+
+static int gc_port_g()
+{
+  return 0
+#undef G
+#define G(abspin, relpin) \
+  | (gpio_pin_in_use(abspin) << (relpin))
+#include "arch/stm32l4xxx/peripherals/tables/stm32l432xx/gpio/port_table.inc"
+    ;
+#undef G
+#define G(...)
+}
+
+static int gc_port_h()
+{
+  return 0
+#undef H
+#define H(abspin, relpin) \
+  | (gpio_pin_in_use(abspin) << (relpin))
+#include "arch/stm32l4xxx/peripherals/tables/stm32l432xx/gpio/port_table.inc"
+    ;
+#undef H
+#define H(...)
+}
+
+static int gc_port_i()
+{
+  return 0
+#undef I
+#define I(abspin, relpin) \
+  | (gpio_pin_in_use(abspin) << (relpin))
+#include "arch/stm32l4xxx/peripherals/tables/stm32l432xx/gpio/port_table.inc"
+    ;
+#undef I
+#define I(...)
+}
+
+
+static inline bool gpio_pin_try_reserve(gpio_pin_t pin)
+{
+  int in_use = __sync_fetch_and_or(
+      &pins_inuse[pin / 32], 1 << (pin % 32));
+  return !(in_use & (1 << (pin % 32)));
+}
+
+inline static gpio_port_config_t* get_gpio_port_config(gpio_port_t port)
+{
+  switch(port) {
+    case GPIO_PORT_A: return (gpio_port_config_t*) GPIOA_BASE;
+    case GPIO_PORT_B: return (gpio_port_config_t*) GPIOB_BASE;
+    case GPIO_PORT_C: return (gpio_port_config_t*) GPIOC_BASE;
+    case GPIO_PORT_H: return (gpio_port_config_t*) GPIOH_BASE;
+    default: return NULL;
+  }
+}
+
+inline static gpio_port_config_t* get_gpio_port_config_for_pin(gpio_pin_t pin)
+{
+  gpio_port_t port = get_port_for_pin(pin);
+  return get_gpio_port_config(port);
+}
+
+gpio_reserved_pin_t reserve_gpio_pin(
+    gpio_pin_t pin, gpio_pin_opts_t* opts, int* error_out)
+{
+  *error_out = 0;
+  if (!gpio_pin_try_reserve(pin)) {
+    *error_out = GPIO_ERROR_IN_USE;
+    return (gpio_reserved_pin_t) { .v_ = -1 };
+  }
+
+  gpio_port_t port = get_port_for_pin(pin);
+  regset(RCC.ahb2en_r, rcc_gpioen(port), 1);
+
+  gpio_port_config_t* port_config = get_gpio_port_config(port);
+
+
+  int off = offset_for_gpio_pin(pin);
+
+  regset(port_config->mode_r, gpio_mode_n(off), opts->mode);
+  regset(port_config->pupd_r, gpio_pupd_n(off), opts->pull_dir);
+
+  switch(opts->mode) {
+    case GPIO_MODE_INPUT:
+      break;
+
+    case GPIO_MODE_OUTPUT:
+      regset(port_config->ospeed_r, gpio_ospeed_n(off), opts->output_opts.speed);
+      regset(port_config->otype_r, gpio_otype_n(off), opts->output_opts.type);
+      break;
+
+    case GPIO_MODE_ALTERNATE:
+      if (off < 8) {
+        regset(
+            port_config->af_rl,
+            gpio_afsel_n(off),
+            opts->alternate_opts.function);
+      } else {
+        regset(
+            port_config->af_rh,
+            gpio_afsel_n(off - 8),
+            opts->alternate_opts.function);
+      }
+      break;
+
+    case GPIO_MODE_ANALOG:
+      regset(port_config->asc_r, gpio_asc_n(off), 1);
+      break;
+  }
+
+  return (gpio_reserved_pin_t) { .v_ = pin };
+}
+
+gpio_reserved_pin_t gpio_enable_alternate_function(
+    gpio_alternate_function_t fn,
+    gpio_pin_t hint,
+    int* error_out)
+{
+  int i = 0;
+  gpio_pin_opts_t opts;
+  struct gpio_afn_and_pin afn_and_pin[5];
+
+  if (gpio_pin_out_of_range(hint) && hint != -1) {
+    *error_out = GPIO_ERROR_INVALID_PIN;
+    return (gpio_reserved_pin_t) { .v_ = -1 };
+  }
+
+  opts.mode = GPIO_MODE_ALTERNATE;
+
+  if (fn == GPIO_ALTERNATE_FUNCTION_EVENTOUT) {
+    afn_and_pin[i].afn_number = GPIO_ALTERNATE_FUNCTION_EVENTOUT;
+    if (hint == -1) {
+      hint = GPIO_PIN_PA0;
+    }
+    afn_and_pin[i].gpio_pin = hint;
+  } else {
+     get_ports_and_pins_for_alternate_function(fn, afn_and_pin);
+
+    if (hint == -1) {
+      hint = afn_and_pin[0].gpio_pin;
+    }
+
+    for(i = 0;
+        i < 5
+        && afn_and_pin[i].gpio_pin != hint
+        && afn_and_pin[i].gpio_pin != -1;
+        ++ i);
+
+    if (afn_and_pin[i].gpio_pin == -1 || i == 5) {
+      *error_out = GPIO_ERROR_INVALID_PIN_FOR_ALTERNATE_FUNCTION;
+      return (gpio_reserved_pin_t) { .v_ = -1 };
+    }
+  }
+
+  opts.alternate_opts.function = afn_and_pin[i].afn_number;
+  return reserve_gpio_pin(afn_and_pin[i].gpio_pin, &opts, error_out);
+}
+
+void release_gpio_pin(gpio_reserved_pin_t rpin)
+{
+  gpio_pin_t pin = rpin.v_;
+  // TODO this should be a critical section.
+  gpio_port_t port = get_port_for_pin(pin);
+  pins_inuse[pin / 32] &= ~(1 << (pin % 32));
+
+  int used;
+  switch(port) {
+    case GPIO_PORT_A:
+      used = gc_port_a();
+      break;
+    case GPIO_PORT_B:
+      used = gc_port_b();
+      break;
+    case GPIO_PORT_C:
+      used = gc_port_c();
+      break;
+    case GPIO_PORT_D:
+      used = gc_port_d();
+      break;
+    case GPIO_PORT_E:
+      used = gc_port_e();
+      break;
+    case GPIO_PORT_F:
+      used = gc_port_f();
+      break;
+    case GPIO_PORT_G:
+      used = gc_port_g();
+      break;
+    case GPIO_PORT_H:
+      used = gc_port_h();
+      break;
+    case GPIO_PORT_I:
+      used = gc_port_i();
+      break;
+
+    case N_GPIO_PORTS:
+      used = 1;
+      break;
+  }
+
+  if (!used) {
+    regset(RCC.ahb2en_r, rcc_gpioen(port), 0);
+  }
+}
+
+inline void get_gpio_pin_port_off(
+    gpio_pin_t pin, gpio_port_config_t** out_cfg, int* out_off)
+{
+  *out_cfg = get_gpio_port_config_for_pin(pin);
+  *out_off = offset_for_gpio_pin(pin);
+}
+
+void set_gpio_pin_high(gpio_reserved_pin_t pin)
+{
+  int off;
+  gpio_port_config_t* portcfg;
+  get_gpio_pin_port_off(pin.v_, &portcfg, &off);
+
+  regset(portcfg->od_r, (1 << off), 1);
+}
+
+void set_gpio_pin_low(gpio_reserved_pin_t pin)
+{
+  int off;
+  gpio_port_config_t* portcfg;
+  get_gpio_pin_port_off(pin.v_, &portcfg, &off);
+
+  regset(portcfg->od_r, (1 << off), 0);
+}
diff --git a/src/kern/gpio/sysled.c b/src/kern/gpio/sysled.c
new file mode 100644
index 0000000..a728da3
--- /dev/null
+++ b/src/kern/gpio/sysled.c
@@ -0,0 +1,14 @@
+#include "kern/gpio/sysled.h"
+
+#define SYSLED GPIO_PIN_PB3
+
+gpio_reserved_pin_t get_sysled()
+{
+  if (gpio_pin_in_use(SYSLED)) {
+    return (gpio_reserved_pin_t) { .v_ = SYSLED };
+  }
+  
+  int ec;
+  gpio_pin_opts_t opts = DEFAULT_GPIO_OPTS_OUTPUT;
+  return reserve_gpio_pin(SYSLED, &opts, &ec);
+}
diff --git a/src/kern/init.c b/src/kern/init.c
new file mode 100644
index 0000000..2531ca9
--- /dev/null
+++ b/src/kern/init.c
@@ -0,0 +1,82 @@
+#include "kern/init.h"
+
+#include "arch.h"
+#include "arch/stm32l4xxx/peripherals/system.h"
+#include "arch/stm32l4xxx/peripherals/clock.h"
+
+#include "kern/log.h"
+
+/* Forward-declare the main function. This is implemented in main.c. */
+int main();
+
+/* These are defined in the linker script. */
+
+#ifdef ARCH_STM32L4
+extern uint32_t INIT_DATA_VALUES;
+extern uint32_t DATA_SEGMENT_START;
+extern uint32_t DATA_SEGMENT_STOP;
+extern uint32_t BSS_START;
+extern uint32_t BSS_END;
+
+extern void(*INIT_ROUTINES_FLASH_START)();
+extern void(*INIT_ROUTINES_FLASH_STOP)();
+
+init0()
+{
+  /* Enable a higher clock speed. This is the first thing we do
+   * beacuse it will boost the boot up time. */
+  set_system_clock_MHz(80);
+}
+
+init1()
+{
+  /* Next, we'll copy the data sections from flash to ram. */
+  uint32_t* src;
+  uint32_t* dest;
+
+  src = &INIT_DATA_VALUES;
+  dest = &DATA_SEGMENT_START;
+
+  /* Copy the values from flash into the data segment. */
+  while (dest != &DATA_SEGMENT_STOP) {
+    *(dest++) = *(src++);
+  }
+
+  /* Everything in the BSS segment is set to zero. */
+  dest = &BSS_START;
+  while (dest != &BSS_END) {
+    *(dest++) = 0;
+  }
+}
+
+init3()
+{
+  klogf("--- System Restart ---\n");
+  klogf("Setting the vector offset table to point to the start of flash.\n");
+
+  /* Set the vector offset table to be at the start
+   * of FLASH memory. */
+  SCB.vto_r = 0x08000000;
+}
+
+/*
+ * Runs before main. Initializes the data and bss segments by loading them
+ * into memory.
+ */
+_Noreturn void on_reset()
+{
+  void (**initfn)();
+  for(initfn = &INIT_ROUTINES_FLASH_START;
+      initfn < &INIT_ROUTINES_FLASH_STOP;
+      ++ initfn) {
+
+    (*initfn)();
+  }
+
+  /* Jump to main. */
+  main();
+
+  for(;;);
+}
+
+#endif /* ARCH_STM32L4 */
diff --git a/src/kern/lib.c b/src/kern/lib.c
new file mode 100644
index 0000000..88188cc
--- /dev/null
+++ b/src/kern/lib.c
@@ -0,0 +1,56 @@
+#include "kern/lib.h"
+
+#define nybble_to_hex(n) \
+  ((n) < 10 ? 0x30 + (n) : ('A' + ((n) - 10)))
+
+void hexify(uint32_t v, char* into)
+{
+  into += 8;
+
+  *(into--) = 0;
+
+  *(into--) = nybble_to_hex(v & 0x0F);
+  v >>= 4;
+  *(into--) = nybble_to_hex(v & 0x0F);
+  v >>= 4;
+  *(into--) = nybble_to_hex(v & 0x0F);
+  v >>= 4;
+  *(into--) = nybble_to_hex(v & 0x0F);
+  v >>= 4;
+
+  *(into--) = nybble_to_hex(v & 0x0F);
+  v >>= 4;
+  *(into--) = nybble_to_hex(v & 0x0F);
+  v >>= 4;
+  *(into--) = nybble_to_hex(v & 0x0F);
+  v >>= 4;
+  *into = nybble_to_hex(v & 0x0F);
+  v >>= 4;
+}
+
+void decimalify(int v, char* into)
+{
+  int c = 0;
+  int i;
+
+  if (v == 0) {
+    *(into ++) = '0';
+    *into = 0;
+    return;
+  } else {
+    while (v > 0) {
+      *(into ++) = 0x30 + (v % 10);
+      v /= 10;
+      ++ c;
+    }
+  }
+  *into = 0;
+
+  into -= c;
+  for (i = 0; i < c / 2; ++ i) {
+    char tmp = into[i];
+    into[i] = into[c - i - 1];
+    into[c - i - 1] = tmp;
+  }
+
+}
diff --git a/src/kern/log.c b/src/kern/log.c
new file mode 100644
index 0000000..a217183
--- /dev/null
+++ b/src/kern/log.c
@@ -0,0 +1,55 @@
+#include "arch/stm32l4xxx/peripherals/usart.h"
+#include "arch/stm32l4xxx/peripherals/clock.h"
+
+#include "kern/log.h"
+#include "kern/init.h"
+#include "kern/gpio/gpio_manager.h"
+
+#include "kern/common.h"
+
+void setup_usart2(uint32_t baud_rate);
+
+/** This module requires an initialization routine. This is a level2 routine,
+ * so anything running at level3 or lower is guaranteed to have access
+ * to the klong. */
+init2()
+{
+  setup_usart2(115200);
+  regset(USART2.c_r1, usart_txeie, 1);
+  regset(USART2.c_r1, usart_rxneie, 1);
+  usart_set_enabled(&USART2, USART_ENABLE_TX | USART_ENABLE_RX);
+
+  klogf("klog() enabled on USART2\n");
+}
+
+void klogf(const char* fmt, ...)
+{
+  va_list l;
+  va_start(l, fmt);
+
+  usart_vprintf(&USART2, fmt, l);
+}
+
+void setup_usart2(uint32_t baud_rate)
+{
+  enable_hsi(&RCC, true);
+
+  int ec = 0;
+  gpio_enable_alternate_function(
+      GPIO_ALTERNATE_FUNCTION_USART2_TX, GPIO_PIN_PA2, &ec);
+
+  gpio_enable_alternate_function(
+      GPIO_ALTERNATE_FUNCTION_USART2_RX, GPIO_PIN_PA15, &ec);
+
+  set_usart2_clock_src(&RCC, USART_CLK_SRC_HSI16);
+  set_usart2_clock_enabled(&RCC, USART_CLK_SRC_HSI16);
+
+  /* De-assert reset of USART2 */
+  regset(RCC.apb1rst1_r, rcc_usart2rst, 0);
+
+  USART2.c_r1 = 0;
+  USART2.c_r2 = 0;
+  USART2.c_r3 = 0;
+
+  usart_set_divisor(&USART2, 16000000 / baud_rate);
+}
diff --git a/src/kern/main.c b/src/kern/main.c
new file mode 100644
index 0000000..ebb2164
--- /dev/null
+++ b/src/kern/main.c
@@ -0,0 +1,29 @@
+#include "arch.h"
+#include "kern/log.h"
+
+#include "arch/stm32l4xxx/peripherals/system.h"
+#include "arch/stm32l4xxx/peripherals/clock.h"
+
+void on_systick() /* Overrides weak-symbol on_systick. */
+{
+  klogf("Systick\n");
+}
+
+#ifdef ARCH_STM32L4
+
+/* Main function. This gets executed from the interrupt vector defined above. */
+int main()
+{
+  klogf("Hello, World! Clock Mhz: %d\n", (uint32_t) get_clock_mhz());
+
+  /* Set the countdown to start from 10,000,0000. */
+  SCB.strv_r = 10000000;
+
+  /* Enable interrupts. */
+  regset(SCB.stcs_r, scb_tickint, 1);
+
+  /* Start the systick. */
+  regset(SCB.stcs_r, scb_enable, 1);
+}
+
+#endif
diff --git a/src/kern/mem.c b/src/kern/mem.c
new file mode 100644
index 0000000..5234fff
--- /dev/null
+++ b/src/kern/mem.c
@@ -0,0 +1,280 @@
+#include "arch.h"
+#include "kern/mem.h"
+#include "kern/common.h"
+
+#ifdef ARCH_STM32L4
+/* Provide a definition for memset() when not provided for the
+ * microcontroller. */
+void* memset(void* dest, int c, size_t n)
+{
+  uint8_t c8 = (uint8_t) c;
+  uint8_t* dest8 = (uint8_t*) dest;
+  uint8_t* to = dest8 + n;
+
+  while(dest8 < to) {
+    *(dest8 ++) = c8;
+  }
+
+  return dest;
+}
+
+#else
+
+void* memset(void* dest, int c, size_t n);
+
+#endif
+
+typedef uint16_t halloc_off_t;
+
+#define CANARY 0x5a
+
+// The sizes will count the number of WORDS allocated.
+// Since there's a max size of 16k, only 12 bits will be
+// needed for this.
+typedef struct HALLOC_NODE {
+  union {
+    uint32_t header;
+    struct {
+      /* Is this memory block currently in use (hasn't been hfree'd) */
+      uint8_t     used:1;
+      /* Number of words allocated. Does not include the header. */
+      uint16_t size:12;
+      /* The location of the previous block (in WORDS from offest) */
+      halloc_off_t prev:12;
+      uint8_t canary:7;
+    } PACKED;
+  };
+
+  uint8_t mem[]; /* The memory to use. */
+} halloc_node_t;
+
+static_assert(offsetof(halloc_node_t, mem) == 4, "Offset check failed.");
+
+halloc_node_t* halloc_start;
+
+#define halloc_node_out_of_range(node) \
+   ((uint8_t*) (node) == ((uint8_t*)&DATA_SEGMENT_STOP) + MAX_HEAP_SIZE)
+
+#define halloc_node_next(cur) \
+  ((halloc_node_t*)(((uint8_t*)(cur)) + (((cur)->size + 1) * 4)))
+
+#define halloc_node_prev(cur) halloc_node_at_off(cur->prev)
+
+#define halloc_node_at_off(offset) \
+  ((halloc_node_t*)(((uint8_t*) halloc_start) + (offset) * 4))
+
+#define halloc_node_get_off(node) \
+  (((uint32_t)(((uint8_t*)(node)) - ((uint8_t*)(halloc_start)))) / 4)
+
+#define get_halloc_node(mem) \
+  ((halloc_node_t*)(((uint8_t*)mem) - 4))
+
+#define size_for(n) \
+  (((n) / 4) + ((n) % 4 != 0))
+
+void* halloc(size_t size)
+{
+  if (!halloc_start) {
+    halloc_start = (halloc_node_t*) DATA_SEGMENT_STOP_ADDR;
+    memset(halloc_start, 0, sizeof(halloc_node_t));
+    halloc_start->size = (MAX_HEAP_SIZE / 4) - 1;
+    halloc_start->canary = CANARY;
+  }
+
+  size_t realsz = size_for(size); /* Clip the size to the nearest word. */
+  halloc_off_t offset = 0; 
+  while (offset < (MAX_HEAP_SIZE / 4)) {
+    halloc_node_t* cur = halloc_node_at_off(offset);
+
+    if (!cur->used && (cur->size >= realsz)) {
+      cur->used = true;
+      size_t orig_size = cur->size;
+      cur->size = realsz;
+      
+      if (orig_size > realsz) {
+        /* This halloc node needs to split into two blocks. */
+        halloc_node_t* next = halloc_node_next(cur);
+        next->used = 0;
+        next->size = orig_size - realsz - sizeof(halloc_node_t) / 4;
+        next->prev = offset;
+        next->canary = CANARY;
+
+        halloc_node_t* nextnext = halloc_node_next(next);
+        if (halloc_node_get_off(nextnext) < (MAX_HEAP_SIZE / 4)) {
+          nextnext->prev = halloc_node_get_off(next);
+        }
+      }
+
+      return (void*) cur->mem;
+    }
+
+    offset += (sizeof(halloc_node_t) / 4) + cur->size;
+  }
+
+  return NULL;
+}
+
+
+/* Joins this node with the previous and next nodes if they're free. */
+static void coalesce(halloc_node_t* cur)
+{
+  halloc_node_t* orig = cur;
+  halloc_node_t* last_freed;
+  halloc_node_t* next_used;
+
+  /* Find the earliest contiguous free'd block. */
+  while (!cur->used && cur != halloc_start) {
+    cur = halloc_node_prev(cur);
+  }
+
+  if (cur == halloc_start && !cur->used) {
+    last_freed = cur;
+  } else {
+    last_freed = halloc_node_next(cur);
+  }
+
+  /* Find the next used block. */
+  cur = orig;
+  while (!cur->used && !halloc_node_out_of_range(cur)) {
+    cur = halloc_node_next(cur);
+  }
+
+  next_used = cur;
+
+  if (!halloc_node_out_of_range(next_used)) {
+    next_used->prev = halloc_node_get_off(last_freed);
+  }
+
+  last_freed->size = ((uint8_t*) next_used - (last_freed->mem)) / 4;
+}
+
+#ifdef FOR_TESTING
+#include <assert.h>
+#include <stdio.h>
+void panic(const char* x)
+{
+  fprintf(stderr, "%s\n", x);
+  assert(0);
+}
+#else
+void panic(const char* x)
+{
+  for(;;);
+}
+#endif
+
+void hfree(void* mem)
+{
+  /* Like normal free(), do nothing on free'ing NULL */
+  if (!mem) return;
+
+  halloc_node_t* header = get_halloc_node(mem);
+  if (!header->used) {
+    panic("Heap double free or corruption!\n");
+    return;
+  }
+
+  header->used = 0;
+  coalesce(header);
+}
+
+#ifdef FOR_TESTING
+
+#include <stdio.h>
+
+void* debug_halloc_get_next_ptr(void* ptr)
+{
+  halloc_node_t* node = ptr - sizeof(halloc_node_t);
+  halloc_node_t* next = halloc_node_next(node);
+  
+  return next->mem;
+}
+
+void* debug_halloc_get_prev_ptr(void* ptr)
+{
+  halloc_node_t* node = ptr - sizeof(halloc_node_t);
+  halloc_node_t* prev = halloc_node_prev(node);
+  
+  return prev->mem;
+}
+
+void debug_print_blocks()
+{
+  printf("------ Print Blocks -------\n");
+  halloc_node_t* cur = halloc_node_at_off(0);
+
+  while (!halloc_node_out_of_range(cur)) {
+    printf("header (%04x) {used=%d, size=%5d, prev=%04x, canary=%02x}\n",
+        halloc_node_get_off(cur), cur->used, cur->size, cur->prev, cur->canary);
+    cur = halloc_node_next(cur);
+  }
+}
+
+/* Tests that we can walk up and down the allocated blocks and that they
+ * are properly aligned. */
+int debug_halloc_assert_consistency(char* error, size_t len)
+{
+  halloc_node_t* cur = halloc_node_at_off(0);
+  size_t total_size = 0;
+  size_t loop_check = 0;
+  size_t n_blocks = 1;
+  size_t n_blocks_back = 1;
+
+  while(1) {
+    if (cur->canary != CANARY) {
+      snprintf(error, len, "Node has corrupted canary. %02x vs expected %02x\n",
+          cur->canary, CANARY);
+      return 1;
+    }
+
+    total_size += cur->size + 1;
+
+    halloc_node_t* next = halloc_node_next(cur);
+    if ((uint8_t*) next == ((uint8_t*)&DATA_SEGMENT_STOP) + MAX_HEAP_SIZE) {
+      break;
+    } else if ((uint8_t*) next > (uint8_t*)DATA_SEGMENT_STOP_ADDR + MAX_HEAP_SIZE){
+      snprintf(
+          error, len, "Next node points is out of bounds. %p vs max of %p\n",
+          next,
+          (void*)(DATA_SEGMENT_STOP_ADDR + MAX_HEAP_SIZE));
+      return 1;
+    }
+
+    cur = next;
+    ++ n_blocks;
+  }
+
+  if (total_size * 4 != MAX_HEAP_SIZE) {
+    snprintf(
+        error, len, "Total recorded size is inconsistent. %lu vs %lu\n",
+        total_size * 4, MAX_HEAP_SIZE);
+    return 1;
+  }
+
+  if (cur == halloc_start) {
+    return 0;
+  }
+
+  while (loop_check < 10000) {
+    halloc_node_t* prev = halloc_node_prev(cur);
+    ++ n_blocks_back;
+
+    if (prev == halloc_start)  {
+      if (n_blocks != n_blocks_back) {
+        snprintf(
+            error, len, "Different number of blocks found on the way back. Found %lu on the way back vs %lu up.\n",
+            n_blocks_back, n_blocks);
+        return 1;
+      }
+      return 0;
+    }
+
+    cur = prev;
+    ++ loop_check;
+  }
+
+  snprintf(error, len, "Loop check failed.\n");
+  return 1;
+}
+
+#endif
diff --git a/src/kern/stdlibrepl.c b/src/kern/stdlibrepl.c
new file mode 100644
index 0000000..2d9d839
--- /dev/null
+++ b/src/kern/stdlibrepl.c
@@ -0,0 +1,13 @@
+/*
+ * Replacement for common stdlib functions that don't exist
+ * on the ARM bare-metal compilation environment.
+ */
+
+#include <stddef.h>
+
+size_t strlen(char* ch)
+{
+  size_t ret = 0;
+  while(*(ch ++) != 0) ++ ret;
+  return ret;
+}
diff --git a/src/kern/string.c b/src/kern/string.c
new file mode 100644
index 0000000..4afa228
--- /dev/null
+++ b/src/kern/string.c
@@ -0,0 +1,9 @@
+#include "kern/string.h"
+
+void kstrcpy(char* into, const char* from)
+{
+  while(*from) {
+    *(into ++) = *(from ++);
+  }
+  *into = 0;
+}
diff --git a/src/kern/vector.c b/src/kern/vector.c
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/src/kern/vector.c