Added licensing. Moved lots of header files around. Added HardwareSerial reads, writes. Tweaked some of the util and assert functions. Added various useful routines for printing stuff to the serial port. Continued moving out stm32lib. Slightly more consistent naming, this will come in another change.

git-svn-id: https://leaflabs.googlecode.com/svn/trunk/library@74 749a229e-a60e-11de-b98f-4500b42dc123

20 files changed, 991 insertions, 195 deletions

diff --git a/src/lib/adc.c b/src/lib/adc.c
index bcdf874..7169824 100644
--- a/src/lib/adc.c
+++ b/src/lib/adc.c
@@ -1,3 +1,28 @@
+/* *****************************************************************************
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Created: 12/18/09 02:34:47
+ *  Copyright (c) 2009 Perry L. Hung. All rights reserved.
+ *
+ * ****************************************************************************/
+
+/**
+ *  @file adc.c
+ *
+ *  @brief Analog to digital converter routines
+ */
+
 #include "stm32f10x_rcc.h"
 #include "adc.h"
 #include <stdio.h>
@@ -49,6 +74,7 @@ void adc_init(void) {
     ADC_SQR1 = 0;
 
     /* Up the sample conversion time to 55.5 cycles/sec, see note above  */
+    /* TODO: fix magic numbers  */
     ADC_SMPR1 = 0xB6DB6D;
     ADC_SMPR2 = 0x2DB6DB6D;
 
@@ -66,3 +92,6 @@ void adc_init(void) {
 }
 
 
+void adc_disable(void) {
+    CR2_ADON_BIT = 0;
+}
diff --git a/src/lib/adc.h b/src/lib/adc.h
index 41f975e..d554b02 100644
--- a/src/lib/adc.h
+++ b/src/lib/adc.h
@@ -1,3 +1,28 @@
+/* *****************************************************************************
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Created: 12/18/09 02:35:10
+ *  Copyright (c) 2009 Perry L. Hung. All rights reserved.
+ *
+ * ****************************************************************************/
+
+/**
+ *  @file adc.h
+ *
+ *  @brief ADC prototypes and defines
+ */
+
 #ifndef _ADC_H_
 #define _ADC_H_
 #include <inttypes.h>
@@ -40,6 +65,7 @@ extern "C"{
 
 /* Initialize ADC1 to do one-shot conversions  */
 void adc_init(void);
+void adc_disable(void);
 
 /* Perform a single conversion on ADC[0-16],
  * PRECONDITIONS:
diff --git a/src/lib/exti.c b/src/lib/exti.c
index 8e46bb1..bdaa204 100644
--- a/src/lib/exti.c
+++ b/src/lib/exti.c
@@ -1,5 +1,31 @@
+/* *****************************************************************************
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Created: 12/18/09 02:35:22
+ *  Copyright (c) 2009 Perry L. Hung. All rights reserved.
+ *
+ * ****************************************************************************/
+
+/**
+ *  @file exti.c
+ *
+ *  @brief External interrupt control routines
+ */
+
+#include "libmaple.h"
 #include "exti.h"
-#include "util.h"
+#include "nvic.h"
 
 volatile static voidFuncPtr exti_handlers[NR_EXTI_CHANNELS];
 
@@ -22,7 +48,7 @@ void EXTI0_IRQHandler(void) {
         exti_handlers[EXTI0]();
     }
 
-    /* Clear pending bit*/    
+    /* Clear pending bit*/
     clear_pending(EXTI0);
 }
 
@@ -33,7 +59,7 @@ void EXTI1_IRQHandler(void) {
         exti_handlers[EXTI1]();
     }
 
-    /* Clear pending bit*/    
+    /* Clear pending bit*/
     clear_pending(EXTI1);
 }
 
@@ -44,7 +70,7 @@ void EXTI2_IRQHandler(void) {
         exti_handlers[EXTI2]();
     }
 
-    /* Clear pending bit*/    
+    /* Clear pending bit*/
     clear_pending(EXTI2);
 }
 
@@ -55,7 +81,7 @@ void EXTI3_IRQHandler(void) {
         exti_handlers[EXTI3]();
     }
 
-    /* Clear pending bit*/    
+    /* Clear pending bit*/
     clear_pending(EXTI3);
 }
 
@@ -66,7 +92,7 @@ void EXTI4_IRQHandler(void) {
         exti_handlers[EXTI4]();
     }
 
-    /* Clear pending bit*/    
+    /* Clear pending bit*/
     clear_pending(EXTI4);
 }
 
@@ -80,6 +106,7 @@ void EXTI9_5_IRQHandler(void) {
     /* Dispatch every handler if the pending bit is set */
     for (i = 0; i < 5; i++) {
         if (pending & 0x1) {
+            ASSERT(exti_handlers[EXTI5 + i]);
             exti_handlers[EXTI5 + i]();
             clear_pending(EXTI5 + i);
         }
@@ -97,6 +124,7 @@ void EXTI15_10_IRQHandler(void) {
     /* Dispatch every handler if the pending bit is set */
     for (i = 0; i < 6; i++) {
         if (pending & 0x1) {
+            ASSERT(exti_handlers[EXTI10 + i]);
             exti_handlers[EXTI10 + i]();
             clear_pending(EXTI10 + i);
         }
@@ -110,6 +138,7 @@ void exti_attach_interrupt(uint8_t channel, uint8_t port, voidFuncPtr handler, u
     ASSERT(port < NR_EXTI_PORTS);
     ASSERT(mode < NR_EXTI_MODES);
     ASSERT(EXTI0 == 0);
+    ASSERT(handler);
 
     /* Note: All of the following code assumes that EXTI0 = 0  */
 
@@ -128,7 +157,7 @@ void exti_attach_interrupt(uint8_t channel, uint8_t port, voidFuncPtr handler, u
     case EXTI7:
         REG_SET_MASK(AFIO_EXTICR2, BIT_MASK_SHIFT(port, (channel-4)*4));
         break;
-        
+
     case EXTI8:
     case EXTI9:
     case EXTI10:
@@ -225,7 +254,7 @@ void exti_detach_interrupt(uint8_t channel) {
     case EXTI7:
     case EXTI8:
     case EXTI9:
-        /* Are there any other channels enabled? 
+        /* Are there any other channels enabled?
          * If so, don't disable the interrupt handler */
         if (GET_BITS(REG_GET(EXTI_IMR), 5, 9) == 0) {
             REG_SET(NVIC_ICER0, BIT(23));
@@ -237,7 +266,7 @@ void exti_detach_interrupt(uint8_t channel) {
     case EXTI13:
     case EXTI14:
     case EXTI15:
-        /* Are there any other channels enabled? 
+        /* Are there any other channels enabled?
          * If so, don't disable the interrupt handler */
         if (GET_BITS(REG_GET(EXTI_IMR), 10, 15) == 0) {
             REG_SET(NVIC_ICER1, BIT(8));
diff --git a/src/lib/exti.h b/src/lib/exti.h
index ccae3a4..c728454 100644
--- a/src/lib/exti.h
+++ b/src/lib/exti.h
@@ -1,3 +1,28 @@
+/* *****************************************************************************
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Created: 12/18/09 02:35:33
+ *  Copyright (c) 2009 Perry L. Hung. All rights reserved.
+ *
+ * ****************************************************************************/
+
+/**
+ *  @file exti.h
+ *
+ *  @brief External interrupt control prototypes and defines
+ */
+
 #ifndef _EXTI_H_
 #define _EXTI_H_
 
@@ -26,14 +51,14 @@
  *
  * RM0008, page 107: "PD0, PD1 cannot be used for external interrupt/event generation
  * on 36, 48, 64-bin packages."
- * 
+ *
  * ----------------------------------------------------------------------------
  * Pin to EXTI Line Mappings:
- * EXTI0          EXTI1          EXTI2           EXTI3             EXTI4  
+ * EXTI0          EXTI1          EXTI2           EXTI3             EXTI4
  * ----------------------------------------------------------------------------
  * D2/PA0         D3/PA1         D1/PA2          D0/A6/PA3         D10/A10/PA4
  * D26/EXT7/PB0   D27/EXT8/PB1   D16/A2/PC2      D17/A3/PC3        D18/A4/PC4
- * D14/A0/PC0     D15/PC1        D25/EXT5/PD2 
+ * D14/A0/PC0     D15/PC1        D25/EXT5/PD2
  *
  * EXTI5          EXTI6          EXTI7           EXTI8             EXTI9
  * ----------------------------------------------------------------------------
@@ -53,7 +78,7 @@
  * D22/EXT3/PC15
  *
  *
- * The 16 EXTI interrupts are mapped to 7 interrupt handlers. 
+ * The 16 EXTI interrupts are mapped to 7 interrupt handlers.
  *
  * EXTI Lines to Interrupt Mapping:
  * EXTI0 -> EXTI0
@@ -64,7 +89,7 @@
  * EXTI[5-9] -> EXT9_5
  * EXTI[10-15] -> EXT15_10
  *
- * 
+ *
  * */
 
 #define NR_EXTI_CHANNELS         16
@@ -84,21 +109,6 @@
 #define AFIO_EXTICR3    (AFIO_EVCR + 0x10)
 #define AFIO_EXTICR4    (AFIO_EVCR + 0x14)
 
-#define NVIC_EXTI1_OFFSET   (NVIC_ISER0 + 0x07)
-#define NVIC_EXTI9_5_OFFSET (NVIC_ISER0 + 0x17)
-
-/* NVIC Interrupt Enable registers  */
-#define NVIC_ISER0          0xE000E100
-#define NVIC_ISER1          0xE000E104
-#define NVIC_ISER2          0xE000E108
-#define NVIC_ISER3          0xE000E10C
-
-/* NVIC Interrupt Clear registers  */
-#define NVIC_ICER0          0xE000E180
-#define NVIC_ICER1          0xE000E184
-#define NVIC_ICER2          0xE000E188
-#define NVIC_ICER3          0xE000E18C
-
 #define EXTI_RISING                     0
 #define EXTI_FALLING                    1
 #define EXTI_RISING_FALLING             2
@@ -125,7 +135,6 @@
 #define EXTI_CONFIG_PORTC 2
 #define EXTI_CONFIG_PORTD 3
 
-typedef void (*voidFuncPtr)(void);
 
 #ifdef __cplusplus
 extern "C"{
diff --git a/src/lib/gpio.c b/src/lib/gpio.c
index db511c4..1e42d4d 100644
--- a/src/lib/gpio.c
+++ b/src/lib/gpio.c
@@ -1,7 +1,35 @@
-#include "wiring.h"
+/* *****************************************************************************
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Created: 12/18/09 02:35:49
+ *  Copyright (c) 2009 Perry L. Hung. All rights reserved.
+ *
+ * ****************************************************************************/
+
+/**
+ *  @file gpio.c
+ *
+ *  @brief GPIO initialization routine
+ */
+
+#ifndef _GPIO_H_
+#define _GPIO_H_
+
+#endif
+#include "libmaple.h"
 #include "stm32f10x_rcc.h"
 #include "gpio.h"
-#include "util.h"
 
 void gpio_init(void) {
    /* Turn on clocks for GPIO  */
@@ -12,3 +40,19 @@ void gpio_init(void) {
                           ENABLE);
 }
 
+void gpio_set_mode(GPIO_Port* port, uint8_t gpio_pin, uint8_t mode) {
+    uint32_t tmp;
+    uint32_t shift = POS(gpio_pin % 8);
+    GPIOReg CR;
+
+    ASSERT(port);
+    ASSERT(gpio_pin < 16);
+
+    CR = (gpio_pin < 8) ? &(port->CRL) : &(port->CRH);
+
+    tmp = *CR;
+    tmp &= POS_MASK(shift);
+    tmp |= mode << shift;
+
+    *CR = tmp;
+}
diff --git a/src/lib/gpio.h b/src/lib/gpio.h
index 7620f96..bc9bdde 100644
--- a/src/lib/gpio.h
+++ b/src/lib/gpio.h
@@ -1,9 +1,30 @@
-#ifndef _GPIO_H
-#define _GPIO_H
+/* *****************************************************************************
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Created: 12/18/09 02:36:01
+ *  Copyright (c) 2009 Perry L. Hung. All rights reserved.
+ *
+ * ****************************************************************************/
 
-#include <inttypes.h>
-#include "util.h"
+/**
+ *  @file gpio.h
+ *
+ *  @brief GPIO prototypes, defines, and inlined access functions
+ */
 
+#ifndef _GPIO_H
+#define _GPIO_H
 
 /* Each of the GPIO port bits can be in the following modes (STM32 138/995):
  *      Input floating
@@ -13,15 +34,15 @@
  *      Output open-drain
  *      Output push-pull
  *      Alternate function push-pull
- *      Alternate function open-drain 
+ *      Alternate function open-drain
  *
  * - After reset, the alternate functions are not active and IO prts
  * are set to Input Floating mode */
 
-#define _GPIOA_BASE               (GPIO_Port*)0x40010800
-#define _GPIOB_BASE               (GPIO_Port*)0x40010C00
-#define _GPIOC_BASE               (GPIO_Port*)0x40011000
-#define _GPIOD_BASE               (GPIO_Port*)0x40011400
+#define GPIOA_BASE               (GPIO_Port*)0x40010800
+#define GPIOB_BASE               (GPIO_Port*)0x40010C00
+#define GPIOC_BASE               (GPIO_Port*)0x40011000
+#define GPIOD_BASE               (GPIO_Port*)0x40011400
 
 /* Pin modes are set by [CNFx[1:0] : MODEx[1:0]] */
 #define GPIO_SPEED_50MHZ            (0x3) // Max output speed 50 MHz
@@ -37,13 +58,13 @@
 #define GPIO_MODE_INPUT_PD          (0x02 << 2)
 #define GPIO_MODE_INPUT_PU          (0x02 << 2)
 
-#define INPUT_ANALOG                GPIO_MODE_INPUT_ANALOG
-#define INPUT_DIGITAL               GPIO_MODE_INPUT_FLOATING
-#define INPUT_FLOATING              GPIO_MODE_INPUT_FLOATING
-#define INPUT_PULLDOWN              GPIO_MODE_INPUT_PD
-#define INPUT_PULLUP                GPIO_MODE_INPUT_PU
-#define INPUT                       GPIO_MODE_INPUT_FLOATING
-#define OUTPUT                      GPIO_MODE_OUTPUT_PP
+//#define INPUT_ANALOG                GPIO_MODE_INPUT_ANALOG
+//#define INPUT_DIGITAL               GPIO_MODE_INPUT_FLOATING
+//#define INPUT_FLOATING              GPIO_MODE_INPUT_FLOATING
+//#define INPUT_PULLDOWN              GPIO_MODE_INPUT_PD
+//#define INPUT_PULLUP                GPIO_MODE_INPUT_PU
+//#define INPUT                       GPIO_MODE_INPUT_FLOATING
+//#define OUTPUT                      GPIO_MODE_OUTPUT_PP
 
 typedef struct {
     volatile uint32_t CRL;      // Port configuration register low
@@ -65,23 +86,7 @@ extern "C"{
 #endif
 
 void gpio_init(void);
-
-static inline void gpio_set_mode(GPIO_Port* port, uint8_t gpio_pin, uint8_t mode) {
-    uint32_t tmp;
-    uint32_t shift = POS(gpio_pin % 8);
-    GPIOReg CR;
-
-    ASSERT(port);
-    ASSERT(gpio_pin < 16);
-
-    CR = (gpio_pin < 8) ? &(port->CRL) : &(port->CRH);
-
-    tmp = *CR;
-    tmp &= POS_MASK(shift);
-    tmp |= mode << shift;
-
-    *CR = tmp;
-}
+void gpio_set_mode(GPIO_Port* port, uint8_t gpio_pin, uint8_t mode);
 
 static inline void gpio_write_bit(GPIO_Port *port, uint8_t gpio_pin, uint8_t val) {
     if (val){
diff --git a/src/lib/libmaple.h b/src/lib/libmaple.h
new file mode 100644
index 0000000..dc9ffd2
--- /dev/null
+++ b/src/lib/libmaple.h
@@ -0,0 +1,33 @@
+/* *****************************************************************************
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Created: 12/19/09 02:37:22 EST
+ *  Copyright (c) 2009 Perry L. Hung. All rights reserved.
+ *
+ * ****************************************************************************/
+
+/**
+ *  @file libmaple.h
+ *
+ *  @brief general include file for libmaple
+ */
+
+#ifndef _LIBMAPLE_H_
+#define _LIBMAPLE_H_
+
+#include "libmaple_types.h"
+#include "util.h"
+
+#endif
+
diff --git a/src/lib/libmaple_types.h b/src/lib/libmaple_types.h
new file mode 100644
index 0000000..b798587
--- /dev/null
+++ b/src/lib/libmaple_types.h
@@ -0,0 +1,42 @@
+/* *****************************************************************************
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Created: 12/19/09 02:35:14 EST
+ *  Copyright (c) 2009 Perry L. Hung. All rights reserved.
+ *
+ * ****************************************************************************/
+
+/**
+ *  @file libmaple_types.h
+ *
+ *  @brief libmaple types
+ */
+
+#ifndef _LIBMAPLE_TYPES_H_
+#define _LIBMAPLE_TYPES_H_
+
+typedef unsigned char uint8;
+typedef unsigned short uint16;
+typedef unsigned int uint32;
+typedef unsigned long long uint64;
+
+typedef signed char int8;
+typedef short int16;
+typedef int int32;
+typedef long long int64;
+
+typedef void (*voidFuncPtr)(void);
+
+#endif
+
diff --git a/src/lib/nvic.c b/src/lib/nvic.c
new file mode 100644
index 0000000..61e1d34
--- /dev/null
+++ b/src/lib/nvic.c
@@ -0,0 +1,41 @@
+/* *****************************************************************************
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Created: 12/18/09 02:36:19
+ *  Copyright (c) 2009 Perry L. Hung. All rights reserved.
+ *
+ * ****************************************************************************/
+
+/**
+ *  @file nvic.c
+ *
+ *  @brief Nested interrupt controller routines
+ */
+
+#include "libmaple.h"
+#include "nvic.h"
+#include "systick.h"
+
+void nvic_disable_interrupts(void) {
+    /* Turn off all interrupts  */
+    REG_SET(NVIC_ICER0, 0xFFFFFFFF);
+    REG_SET(NVIC_ICER1, 0xFFFFFFFF);
+
+    /* Turn off systick exception  */
+    REG_CLEAR_BIT(SYSTICK_CSR, 0);
+}
+
+void nvic_set_vector_table(uint32_t *addr, uint32_t offset) {
+//    SCB->VTOR = NVIC_VectTab | (Offset & (u32)0x1FFFFF80);
+}
diff --git a/src/lib/nvic.h b/src/lib/nvic.h
new file mode 100644
index 0000000..a2d5c24
--- /dev/null
+++ b/src/lib/nvic.h
@@ -0,0 +1,62 @@
+/* *****************************************************************************
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Created: 12/18/09 02:36:47
+ *  Copyright (c) 2009 Perry L. Hung. All rights reserved.
+ *
+ * ****************************************************************************/
+
+/**
+ *  @file nvic.h
+ *
+ *  @brief Nested interrupt controller defines and prototypes
+ */
+
+#ifndef _NVIC_H_
+#define _NVIC_H_
+
+#define NVIC_EXTI1_OFFSET   (NVIC_ISER0 + 0x07)
+#define NVIC_EXTI9_5_OFFSET (NVIC_ISER0 + 0x17)
+
+/* NVIC Interrupt Enable registers  */
+#define NVIC_ISER0          0xE000E100
+#define NVIC_ISER1          0xE000E104
+#define NVIC_ISER2          0xE000E108
+#define NVIC_ISER3          0xE000E10C
+
+/* NVIC Interrupt Clear registers  */
+#define NVIC_ICER0          0xE000E180
+#define NVIC_ICER1          0xE000E184
+#define NVIC_ICER2          0xE000E188
+#define NVIC_ICER3          0xE000E18C
+
+/* System control registers  */
+#define SCB_VTOR            0xE000ED08  // Vector table offset register
+
+#define NVIC_VectTab_RAM             ((u32)0x20000000)
+#define NVIC_VectTab_FLASH           ((u32)0x08000000)
+
+#ifdef __cplusplus
+extern "C"{
+#endif
+
+void nvic_disable_interrupts(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif
+
diff --git a/src/lib/syscalls.c b/src/lib/syscalls.c
index 6095bd3..390cecd 100644
--- a/src/lib/syscalls.c
+++ b/src/lib/syscalls.c
@@ -1,5 +1,5 @@
+#include "libmaple.h"
 #include <sys/stat.h>
-#include "stm32f10x_usart.h"
 
 /* _end is set in the linker command file */
 extern caddr_t _end;
@@ -72,8 +72,9 @@ int _lseek(int fd, off_t pos, int whence)
 
 unsigned char getch(void)
 {
-    while (!(USART2->SR & USART_FLAG_RXNE));
-    return USART2->DR;
+//    while (!(USART2->SR & USART_FLAG_RXNE));
+//    return USART2->DR;
+    return 0;
 }
 
 
@@ -86,10 +87,10 @@ int _read(int fd, char *buf, size_t cnt)
 
 void putch(unsigned char c)
 {
-    if (c == '\n') putch('\r');
+//    if (c == '\n') putch('\r');
 
-    while (!(USART2->SR & USART_FLAG_TXE));
-    USART2->DR = c;
+//    while (!(USART2->SR & USART_FLAG_TXE));
+//    USART2->DR = c;
 }
 
 void cgets(char *s, int bufsize)
diff --git a/src/lib/systick.c b/src/lib/systick.c
index f328ae0..47be69e 100644
--- a/src/lib/systick.c
+++ b/src/lib/systick.c
@@ -1,30 +1,52 @@
+/* *****************************************************************************
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Created: 12/18/09 02:37:24
+ *  Copyright (c) 2009 Perry L. Hung. All rights reserved.
+ *
+ * ****************************************************************************/
+
+/**
+ *  @file systick.c
+ *
+ *  @brief System timer interrupt handler and initialization routines
+ */
+
+#include "libmaple.h"
 #include "systick.h"
 
 #define MILLIS_INC 1
 
-volatile uint32_t systick_timer_overflow_count = 0;
 volatile uint32_t systick_timer_millis = 0;
-static uint8_t systick_timer_fract = 0;
 
 void systick_init(void) {
     /* Set the reload counter to tick every 1ms  */
-    SYSTICK_RELOAD = MAPLE_RELOAD_VAL;
+    REG_SET_MASK(SYSTICK_RELOAD, MAPLE_RELOAD_VAL);
+//    SYSTICK_RELOAD = MAPLE_RELOAD_VAL;
 
     /* Clock the system timer with the core clock
      * and turn it on, interrrupt every 1ms to keep track of millis()*/
-    SYSTICK_CSR = SYSTICK_SRC_HCLK |
-                  SYSTICK_ENABLE   |
-                  SYSTICK_TICKINT;
+    REG_SET(SYSTICK_CSR, SYSTICK_SRC_HCLK |
+                         SYSTICK_ENABLE   |
+                         SYSTICK_TICKINT);
+//    SYSTICK_CSR = SYSTICK_SRC_HCLK |
+//                  SYSTICK_ENABLE   |
+//                  SYSTICK_TICKINT;
 }
 
-void SysTickHandler(void)
-{
-   uint32_t m = systick_timer_millis;
-   uint8_t f = systick_timer_fract;
-
-   m += MILLIS_INC;
-   systick_timer_millis = m;
-   systick_timer_overflow_count++;
+void SysTickHandler(void) {
+    systick_timer_millis++;
 }
 
 
diff --git a/src/lib/systick.h b/src/lib/systick.h
index 42d33d0..3d846ef 100644
--- a/src/lib/systick.h
+++ b/src/lib/systick.h
@@ -1,15 +1,40 @@
+/* *****************************************************************************
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Created: 12/18/09 02:37:37
+ *  Copyright (c) 2009 Perry L. Hung. All rights reserved.
+ *
+ * ****************************************************************************/
+
+/**
+ *  @file systick.h
+ *
+ *  @brief Various system timer definitions
+ */
+
 #ifndef _SYSTICK_H_
 #define _SYSTICK_H_
-#include <inttypes.h>
-#include "util.h"
+
+#include "libmaple.h"
 
 /* To the ARM technical manual... there's nearly nothing on the systick
  * timer in the stm32 manual */
 
-#define SYSTICK_CSR       *(volatile int*)0xE000E010  // Control and status register
-#define SYSTICK_RELOAD    *(volatile int*)0xE000E014  // Reload value register
-#define SYSTICK_CNT       *(volatile int*)0xE000E018  // Current value register
-#define SYSTICK_CALIB     *(volatile int*)0xE000E01C  // Calibration value register
+#define SYSTICK_CSR       0xE000E010  // Control and status register
+#define SYSTICK_RELOAD    0xE000E014  // Reload value register
+#define SYSTICK_CNT       0xE000E018  // Current value register
+#define SYSTICK_CALIB     0xE000E01C  // Calibration value register
 
 #define SYSTICK_SRC_HCLK  BIT(2)    // Use core clock
 #define SYSTICK_TICKINT   BIT(1)    // Interrupt on systick countdown
diff --git a/src/lib/timers.c b/src/lib/timers.c
index 401b267..2161d84 100644
--- a/src/lib/timers.c
+++ b/src/lib/timers.c
@@ -1,7 +1,31 @@
+/* *****************************************************************************
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Created: 12/18/09 02:37:54
+ *  Copyright (c) 2009 Perry L. Hung. All rights reserved.
+ *
+ * ****************************************************************************/
+
+/**
+ *  @file timers.c
+ *
+ *  @brief General timer routines
+ */
+
+#include "libmaple.h"
 #include "stm32f10x_rcc.h"
-#include "wiring.h"
 #include "timers.h"
-#include "util.h"
 
 typedef struct {
     volatile uint16_t CR1;
@@ -112,3 +136,19 @@ void timer_init(uint8_t timer_num, uint16_t prescale) {
     timer->EGR  = 1;          // Initialize update event and shadow registers
     timer->CR1  |= 1;         // Enable timer
 }
+
+void timers_disable(void) {
+    Timer *timer;
+    int i;
+    Timer *timers[4] = {
+        (Timer*)TIMER1_BASE,
+        (Timer*)TIMER2_BASE,
+        (Timer*)TIMER3_BASE,
+        (Timer*)TIMER4_BASE,
+    };
+
+    for (i = 0; i < 4; i++) {
+        timer = timers[i];
+        timer->CR1 = 0;
+    }
+}
diff --git a/src/lib/timers.h b/src/lib/timers.h
index 27e5999..9147fd4 100644
--- a/src/lib/timers.h
+++ b/src/lib/timers.h
@@ -1,3 +1,28 @@
+/* *****************************************************************************
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Created: 12/18/09 02:38:10
+ *  Copyright (c) 2009 Perry L. Hung. All rights reserved.
+ *
+ * ****************************************************************************/
+
+/**
+ *  @file timers.h
+ *
+ *  @brief Timer prototypes and various definitions
+ */
+
 /* Note to self:
  * The timer clock frequencies are automatically fixed by hardware.
  * There are two cases:
@@ -91,6 +116,7 @@ typedef volatile uint32_t* TimerCCR;
  *      prescale  ->  {1-65535}
  * */
 void timer_init(uint8_t, uint16_t);
+void timers_disable(void);
 
 /* Turn on PWM with duty_cycle on the specified channel in timer.
  * This function takes in a pointer to the corresponding CCR
diff --git a/src/lib/usart.c b/src/lib/usart.c
index c1a7f43..a06690e 100644
--- a/src/lib/usart.c
+++ b/src/lib/usart.c
@@ -1,44 +1,332 @@
+/* *****************************************************************************
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Created: 12/18/09 02:38:26
+ *  Copyright (c) 2009 Perry L. Hung. All rights reserved.
+ *
+ * ****************************************************************************/
+
+/**
+ *  @file usart.c
+ *
+ *  @brief USART control routines
+ */
+
+#include "libmaple.h"
 #include "stm32f10x_rcc.h"
 #include "usart.h"
+#include "nvic.h"
 
-static inline void usart_putc(usart_port *port, uint8_t ch) {
-    port->DR = ch;
+#define USART1_BASE         0x40013800
+#define USART2_BASE         0x40004400
+#define USART3_BASE         0x40004800
 
-    /* Wait till TXE = 1 */
-    while ((port->SR & USART_TXE) == 0)
-        ;
+#define USART_UE            BIT(13)
+#define USART_M             BIT(12)
+#define USART_TE            BIT(3)
+#define USART_RE            BIT(2)
+#define USART_RXNEIE        BIT(5)       // read data register not empty interrupt enable
+#define USART_TXE           BIT(7)
+#define USART_TC            BIT(6)
+
+#define USART_STOP_BITS_1   BIT_MASK_SHIFT(0b0, 12)
+#define USART_STOP_BITS_05  BIT_MASK_SHIFT(0b01, 12)
+#define USART_STOP_BITS_2   BIT_MASK_SHIFT(0b02, 12)
+#define USART_STOP_BITS_15  BIT_MASK_SHIFT(0b02, 12)
+
+#define USART1_CLK          72000000UL
+#define USART2_CLK          36000000UL
+#define USART3_CLK          36000000UL
+
+#define USART_RECV_BUF_SIZE 64
+
+/* Ring buffer notes:
+ * The buffer is empty when head == tail.
+ * The buffer is full when the head is one byte in front of the tail
+ * The total buffer size must be a power of two
+ * Note, one byte is necessarily left free with this scheme */
+typedef struct usart_ring_buf {
+    uint32 head;
+    uint32 tail;
+    uint8 buf[USART_RECV_BUF_SIZE];
+} usart_ring_buf;
+
+static usart_ring_buf ring_buf1;
+static usart_ring_buf ring_buf2;
+static usart_ring_buf ring_buf3;
+
+typedef struct usart_port {
+    volatile uint32 SR;       // Status register
+    volatile uint32 DR;       // Data register
+    volatile uint32 BRR;      // Baud rate register
+    volatile uint32 CR1;      // Control register 1
+    volatile uint32 CR2;      // Control register 2
+    volatile uint32 CR3;      // Control register 3
+    volatile uint32 GTPR;     // Guard time and prescaler register
+} usart_port;
+
+
+/* Don't overrun your buffer, seriously  */
+void USART2_IRQHandler(void) {
+    /* Read the data  */
+    ring_buf2.buf[ring_buf2.tail++] = (uint8_t)(((usart_port*)(USART2_BASE))->DR);
+    ring_buf2.tail %= USART_RECV_BUF_SIZE;
 }
 
-int32_t usart_init(uint8_t usart_num) {
-    ASSERT((usart_num < NR_USARTS) && (usart_num > 0));
+
+/**
+ *  @brief Enable a USART in single buffer transmission mode, multibuffer
+ *  receiver mode.
+ *
+ *  @param usart_num USART to be initialized
+ *  @param baud Baud rate to be set at
+ *  @param recvBuf buf buffer for receiver
+ *  @param len size of recvBuf
+ *
+ *  @sideeffect Turns on the specified USART
+ */
+void usart_init(uint8 usart_num, uint32 baud) {
+    ASSERT((usart_num <= NR_USARTS) && (usart_num > 0));
+    ASSERT(baud && (baud < USART_MAX_BAUD));
+
     usart_port *port;
-    uint32_t clk_speed;
+    usart_ring_buf *ring_buf;
+
+    uint32 clk_speed;
+    uint32 integer_part;
+    uint32 fractional_part;
+    uint32 tmp;
 
     switch (usart_num) {
     case 1:
-        port = USART1_BASE;
+        port = (usart_port*)USART1_BASE;
+        ring_buf = &ring_buf1;
         clk_speed = USART1_CLK;
         break;
     case 2:
-        port = USART2_BASE;
+        port = (usart_port*)USART2_BASE;
+        ring_buf = &ring_buf2;
         clk_speed = USART2_CLK;
         RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
         break;
     case 3:
-        port = USART3_BASE;
+        port = (usart_port*)USART3_BASE;
+        ring_buf = &ring_buf3;
+        clk_speed = USART3_CLK;
         break;
     default:
         /* should never get here  */
         ASSERT(0);
     }
-    uint32_t baud = 9600;
-    uint32_t usartdiv = clk_speed / baud;
+
+    /* Initialize ring buffer  */
+    ring_buf->head = 0;
+    ring_buf->tail = 0;
 
     /* Set baud rate  */
-    port->BRR = BIT_MASK_SHIFT(B9600_MANTISSA, 4) | B9600_FRACTION;
+    integer_part = ((25 * clk_speed) / (4 * baud));
+    tmp = (integer_part / 100) << 4;
+
+    fractional_part = integer_part - (100 * (tmp >> 4));
+    tmp |= (((fractional_part * 16) + 50) / 100) & ((uint8)0x0F);
+
+    port->BRR = (uint16_t)tmp;
+
+    port->CR1 = USART_TE          |    // transmitter enable
+                USART_RE          |    // receiver enable
+                USART_RXNEIE;          // receive interrupt enable
+
+    /* Turn it on in the nvic  */
+    REG_SET(NVIC_ISER1, BIT(6));
+
+    /* Enable the USART and set mode to 8n1 */
+    port->CR1 |= USART_UE;
+}
+
+
+/**
+ *  @brief Turn off a USART.
+ *
+ *  @param USART to be disabled
+ *
+ *  @sideeffect Turns off the specified USART
+ */
+void usart_disable(uint8 usart_num) {
+    ASSERT((usart_num <= NR_USARTS) && (usart_num > 0));
+    usart_port *port;
+
+    switch (usart_num) {
+    case 1:
+        port = (usart_port*)USART1_BASE;
+        break;
+    case 2:
+        port = (usart_port*)USART2_BASE;
+        break;
+    case 3:
+        port = (usart_port*)USART3_BASE;
+        break;
+    default:
+        /* should never get here  */
+        ASSERT(0);
+    }
+
+    /* Is this usart enabled?  */
+    if (!(port->SR & USART_UE))
+        return;
+
+    /* TC bit must be high before disabling the usart  */
+    while ((port->SR & USART_TC) == 0)
+        ;
+
+    /* Disable UE */
+    port->CR1 = 0;
+}
+
+
+/**
+ *  @brief Print a null terminated string to the specified USART
+ *
+ *  @param[in] usart_num USART to send on
+ *  @param[in] str String to send
+ */
+void usart_putstr(uint8 usart_num, const char* str) {
+    ASSERT((usart_num <= NR_USARTS) && (usart_num > 0));
+    char ch;
+
+    while((ch = *(str++)) != '\0') {
+        usart_putc(usart_num, ch);
+    }
+}
+
+/**
+ *  @brief Print an unsigned integer to the specified usart
+ *
+ *  @param[in] usart_num usart to send on
+ *  @param[in] val number to print
+ */
+void usart_putudec(uint8 usart_num, uint32 val) {
+    ASSERT((usart_num <= NR_USARTS) && (usart_num > 0));
+    char digits[12];
+    int i;
+
+    i = 0;
+    do {
+        digits[i++] = val % 10 + '0';
+        val /= 10;
+    } while (val > 0);
+    while (--i >= 0) {
+        usart_putc(usart_num, digits[i]);
+    }
+
+}
 
-    /* Enable the USART and set 8n1 (M bit clear) enable transmitter*/
-    port->CR1 = USART_UE | USART_TE;
 
-    return 0;
+/**
+ *  @brief Return one character from the receive buffer. Assumes
+ *  that there is data available.
+ *
+ *  @param[in] usart_num number of the usart to read from
+ *
+ *  @return character from ring buffer
+ *
+ *  @sideeffect may update the head pointer of the recv buffer
+ */
+uint8 usart_getc(uint8 usart_num) {
+    uint8 ch;
+    usart_ring_buf *rb;
+
+    switch (usart_num) {
+    case 1:
+        rb = &ring_buf1;
+        break;
+    case 2:
+        rb = &ring_buf2;
+        break;
+    case 3:
+        rb = &ring_buf3;
+        break;
+    default:
+        ASSERT(0);
+    }
+
+    /* Make sure there's actually data to be read  */
+    ASSERT(rb->head != rb->tail);
+
+    /* Read the data and check for wraparound  */
+    ch = rb->buf[rb->head++];
+    rb->head %= USART_RECV_BUF_SIZE;
+
+    return ch;
 }
+
+uint32 usart_data_available(uint8 usart_num) {
+    usart_ring_buf *rb;
+
+    switch (usart_num) {
+    case 1:
+        rb = &ring_buf1;
+        break;
+    case 2:
+        rb = &ring_buf2;
+        break;
+    case 3:
+        rb = &ring_buf3;
+        break;
+    default:
+        ASSERT(0);
+    }
+
+    return rb->tail - rb->head;
+}
+
+
+
+/**
+ *  @brief Output a character out the uart
+ *
+ *  @param[in] usart_num usart number to output on
+ *  @param[in] ch character to send
+ *
+ */
+void usart_putc(uint8 usart_num, uint8 ch) {
+    ASSERT((usart_num <= NR_USARTS) && (usart_num > 0));
+    usart_port *port;
+
+    switch (usart_num) {
+    case 1:
+        port = (usart_port*)USART1_BASE;
+        break;
+    case 2:
+        port = (usart_port*)USART2_BASE;
+        break;
+    case 3:
+        port = (usart_port*)USART3_BASE;
+        break;
+    default:
+        /* Should never get here  */
+        ASSERT(0);
+    }
+
+    if (ch == '\n') {
+        usart_putc(usart_num, '\r');
+    }
+
+    port->DR = ch;
+
+    /* Wait for transmission to complete  */
+    while ((port->SR & USART_TXE) == 0)
+        ;
+}
+
+
diff --git a/src/lib/usart.h b/src/lib/usart.h
index 30f7dc1..4d0c586 100644
--- a/src/lib/usart.h
+++ b/src/lib/usart.h
@@ -1,7 +1,30 @@
+/* *****************************************************************************
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Created: 12/18/09 02:38:35
+ *  Copyright (c) 2009 Perry L. Hung. All rights reserved.
+ *
+ * ****************************************************************************/
+
+/**
+ *  @file usart.h
+ *
+ *  @brief USART Definitions
+ */
+
 #ifndef _USART_H_
 #define _USART_H_
-#include <inttypes.h>
-#include <util.h>
 
 /* Transmit procedure:
  * 1.  Enable the USART by writing the UE bit in USART_CR1 register to 1.
@@ -77,50 +100,24 @@
  * 921600  923.076 2.4375          0.16%     4.875          0.16%
  * 225000  2250    1               0%        2              0%
  *
- * USART_BRR[15:4] = mantissa
- * USART_BRR[3:0] = fraction
- * 111010
- * 0110
  * */
 #define NR_USARTS           0x3
 
-//#define USART1_BASE         0x40013800
-//#define USART2_BASE         0x40004400
-//#define USART3_BASE         0x40004800
-
-#define USART_UE            BIT(13)
-#define USART_M             BIT(12)
-#define USART_TE            BIT(3)
-#define USART_TXE           BIT(7)
-#define USART_STOP_BITS_1   BIT_MASK_SHIFT(0b0, 12)
-#define USART_STOP_BITS_05  BIT_MASK_SHIFT(0b01, 12)
-#define USART_STOP_BITS_2   BIT_MASK_SHIFT(0b02, 12)
-#define USART_STOP_BITS_15  BIT_MASK_SHIFT(0b02, 12)
-
-#define USART1_CLK          72000000UL
-#define USART2_CLK          36000000UL
-#define USART3_CLK          36000000UL
-
-#define B9600_MANTISSA      0xEA
-#define B9600_FRACTION      0x06
-
-
-typedef struct usart_port {
-    volatile uint32_t SR;
-    volatile uint32_t DR;
-    volatile uint32_t BRR;
-    volatile uint32_t CR1;
-    volatile uint32_t CR2;
-    volatile uint32_t CR3;
-    volatile uint32_t GTPR;
-} usart_port;
-
 #ifdef __cplusplus
 extern "C"{
 #endif
 
-int32_t usart_init(uint8_t);
+#define USART_MAX_BAUD      225000
+
+void usart_init(uint8 usart_num, uint32 baud);
+void usart_disable(uint8 usart_num);
+
+void usart_putstr(uint8 usart_num, const char*);
+void usart_putudec(uint8 usart_num, uint32 val);
+void usart_putc(uint8 usart_num, uint8 ch);
 
+uint32 usart_data_available(uint8 usart_num);
+uint8 usart_getc(uint8 usart_num);
 
 #ifdef __cplusplus
 } // extern "C"
diff --git a/src/lib/util.c b/src/lib/util.c
new file mode 100644
index 0000000..c75031f
--- /dev/null
+++ b/src/lib/util.c
@@ -0,0 +1,99 @@
+/* *****************************************************************************
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Created: 12/18/09 02:38:44
+ *  Copyright (c) 2009 Perry L. Hung. All rights reserved.
+ *
+ * ****************************************************************************/
+
+/**
+ *  @file util.h
+ *
+ *  @brief Utility procedures for debugging, mostly an error LED fade and
+ *  messages dumped over a uart for failed asserts.
+ */
+
+#include "libmaple.h"
+#include "usart.h"
+#include "gpio.h"
+#include "nvic.h"
+#include "adc.h"
+#include "timers.h"
+
+#define ERROR_LED_PORT         GPIOA_BASE
+#define ERROR_LED_PIN          5
+#define ERROR_USART_NUM        2
+#define ERROR_USART_BAUD       9600
+#define ERROR_TX_PIN           2
+#define ERROR_TX_PORT          GPIOA_BASE
+
+/* Error assert + fade */
+void _fail(const char* file, int line, const char* exp) {
+    int32_t  slope   = 1;
+    int32_t CC      = 0x0000;
+    int32_t TOP_CNT = 0x02FF;
+    int32_t i       = 0;
+
+    /* Turn off interrupts */
+    nvic_disable_interrupts();
+
+    /* Turn off timers  */
+    timers_disable();
+
+    /* Turn off ADC */
+    adc_disable();
+
+    /* Turn off all usarts  */
+    usart_disable(1);
+    usart_disable(2);
+    usart_disable(3);
+
+    /* Initialize the error usart */
+    gpio_set_mode(ERROR_TX_PORT, ERROR_TX_PIN, GPIO_MODE_AF_OUTPUT_PP);
+    usart_init(ERROR_USART_NUM, ERROR_USART_BAUD);
+
+    /* Print failed assert message  */
+    usart_putstr(ERROR_USART_NUM, "ERROR: FAILED ASSERT(");
+    usart_putstr(ERROR_USART_NUM, exp);
+    usart_putstr(ERROR_USART_NUM, "): ");
+    usart_putstr(ERROR_USART_NUM, file);
+    usart_putstr(ERROR_USART_NUM, ": ");
+    usart_putudec(ERROR_USART_NUM, line);
+    usart_putc(ERROR_USART_NUM, '\n');
+
+    /* Turn on the error LED  */
+    gpio_set_mode(ERROR_LED_PORT, ERROR_LED_PIN, GPIO_MODE_OUTPUT_PP);
+
+    /* Error fade  */
+    while (1) {
+        if (CC == TOP_CNT)  {
+            slope = -1;
+        } else if (CC == 0) {
+            slope = 1;
+        }
+
+        if (i == TOP_CNT)  {
+            CC += slope;
+            i = 0;
+        }
+
+        if (i < CC) {
+            gpio_write_bit(ERROR_LED_PORT, ERROR_LED_PIN, 1);
+        } else {
+            gpio_write_bit(ERROR_LED_PORT, ERROR_LED_PIN, 0);
+        }
+        i++;
+    }
+}
+
diff --git a/src/lib/util.cpp b/src/lib/util.cpp
deleted file mode 100644
index 4eb4fe0..0000000
--- a/src/lib/util.cpp
+++ /dev/null
@@ -1,47 +0,0 @@
-#include "wiring.h"
-#include "Serial.h"
-#include "util.h"
-#include "io.h"
-
-#define ERROR_PIN        13
-
-/* Required for C++ hackery */
-extern "C" void __cxa_pure_virtual(void) {
-    while(1)
-        ;
-}
-
-/* Error assert + fade */
-void _fail(const char* file, int line, const char* exp) {
-    int32_t  slope   = 1;
-    int32_t CC      = 0x0000;
-    int32_t TOP_CNT = 0x02FF;
-    int32_t i       = 0;
-
-    Serial1.print("ERROR: FAILED ASSERT(");
-    Serial1.print(exp);
-    Serial1.print("): ");
-    Serial1.print(file);
-    Serial1.print(":");
-    Serial1.println(line);
-
-    while (1) {
-        if (CC == TOP_CNT)  {
-            slope = -1;
-        } else if (CC == 0) {
-            slope = 1;
-        }
-
-        if (i == TOP_CNT)  {
-            CC += slope;
-            i = 0;
-        }
-
-        if (i < CC) {
-            digitalWrite(ERROR_PIN, HIGH);
-        } else {
-            digitalWrite(ERROR_PIN, LOW);
-        }
-        i++;
-    }
-}
diff --git a/src/lib/util.h b/src/lib/util.h
index 8701af1..e425cc0 100644
--- a/src/lib/util.h
+++ b/src/lib/util.h
@@ -1,3 +1,28 @@
+/* *****************************************************************************
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Created: 12/18/09 02:39:27
+ *  Copyright (c) 2009 Perry L. Hung. All rights reserved.
+ *
+ * ****************************************************************************/
+
+/**
+ *  @file util.h
+ *
+ *  @brief Various macros and utility procedures.
+ */
+
 /* Generally "useful" utility procedures  */
 #ifndef _UTIL_H_
 #define _UTIL_H_