diff --git a/code/Makefile b/code/Makefile
index 1c50664..a668269 100644
--- a/code/Makefile
+++ b/code/Makefile
@@ -108,10 +108,6 @@ include $(CHIBIOS)/os/nil/nil.mk
 include $(CHIBIOS)/os/common/ports/ARMCMx/compilers/GCC/mk/port_v6m.mk
 # Auto-build files in ./source recursively.
 include $(CHIBIOS)/tools/mk/autobuild.mk
-# Other files (optional).
-include $(CHIBIOS)/test/lib/test.mk
-include $(CHIBIOS)/test/nil/nil_test.mk
-include $(CHIBIOS)/test/oslib/oslib_test.mk
 
 # Define linker script file here
 LDSCRIPT= $(STARTUPLD)/STM32F051x8.ld
@@ -119,7 +115,6 @@ LDSCRIPT= $(STARTUPLD)/STM32F051x8.ld
 # C sources that can be compiled in ARM or THUMB mode depending on the global
 # setting.
 CSRC = $(ALLCSRC) \
-       $(TESTSRC) \
        main.c \
        radio/hilbert.c \
        radio/rx.c \
diff --git a/code/drivers/si5351.h b/code/drivers/si5351.h
index 4fa6bed..3c539c4 100644
--- a/code/drivers/si5351.h
+++ b/code/drivers/si5351.h
@@ -24,18 +24,48 @@ struct synth{
   uint8_t phase:7;   //< Phase offset, in increments of 1/(vxco*4) seconds
 };
 
+/** Sets the PLL and Clock in the synth config
+ *
+ * Does not reset the clock setup
+ */
 void synthInit(struct synth * cfg, uint8_t channel, uint8_t pll);
+
+/** Starts the clock output
+ *
+ */
 void synthStart(struct synth * cfg);
+/** Sets the clock to the desired frequency
+ *
+ * Calls synthWriteConfig internally
+ */
 void synthSetCarrier(struct synth * cfg, float carrier);
+/** Adjusts the clock by a certain frequency
+ *
+ * This is much faster than synthSetCarrier, but has limited range.
+ * Also, the frequency may have error due to linearization.
+ *
+ * @param baseband 15.16 fixed-point value, in Hz
+ */
 void synthSetBaseband(struct synth * cfg, int32_t baseband);
+/** Writes a and b parameters for a pll multiplier or divider
+ */
 void synthWriteParam(uint8_t reg, uint64_t val, uint8_t div);
+/** Writes config to device
+ *
+ * This may be useful for restoring configurations without recalculating
+ */
 void synthWriteConfig(struct synth * cfg);
-/* Sets the initial phase offset
+/** Sets the initial phase offset
  *
  * Note that this has limited resolution and range. The maximum delay is 31.75x the PLL clock period,
  * which means you will have a difficult time getting a 90-degree phase shift on the 80m band.
  */
 void synthSetPhase(struct synth * cfg, float degrees);
+/** Stops the clock output
+ *
+ * This may be helpful for power saving
+ * Unless otherwise configured, clock output will be held low while stopped
+ */
 void synthStop(struct synth * cfg);
 
 
diff --git a/code/main.c b/code/main.c
index b86aac2..3bb8e68 100644
--- a/code/main.c
+++ b/code/main.c
@@ -16,8 +16,6 @@
 
 #include "hal.h"
 #include "ch.h"
-#include "nil_test_root.h"
-#include "oslib_test_root.h"
 #include "drivers/si5351.h"
 
 static void adcerrorcallback(ADCDriver *adcp, adcerror_t err) {
@@ -133,12 +131,7 @@ THD_FUNCTION(Thread3, arg) {
   /* Welcome message.*/
   chnWrite(&SD1, (const uint8_t *)"Hello World!\r\n", 14);
 
-  /* Waiting for encoder turn and activation of the test suite.*/
   while (true) {
-    if (palReadLine(LINE_ENC0)) {
-      test_execute((BaseSequentialStream *)&SD1, &nil_test_suite);
-      test_execute((BaseSequentialStream *)&SD1, &oslib_test_suite);
-    }
     chThdSleepMilliseconds(500);
   }
 }
diff --git a/code/radio/hilbert.c b/code/radio/hilbert.c
index bdf13a9..cd0f019 100644
--- a/code/radio/hilbert.c
+++ b/code/radio/hilbert.c
@@ -7,7 +7,7 @@
 
 #include "hilbert.h"
 
-int16_t hilbert19(int16_t * src){
+int32_t hilbert19(int16_t * src){
   const size_t M = 19/2;
   /* Hilbert coeffecients with a hamming window
    * Values are 2/pi, 2/3pi, 2/7pi, 2/9pi, etc
@@ -20,5 +20,25 @@ int16_t hilbert19(int16_t * src){
   sum += (src[M-5]-src[M+5]) * (int32_t)coef[2];
   sum += (src[M-7]-src[M+7]) * (int32_t)coef[3];
   sum += (src[M-9]-src[M+9]) * (int32_t)coef[4];
-  return sum>>16;
+  return sum;
+}
+
+int32_t hilbert32(int16_t * src, uint8_t i){
+  i -= 16; // Move to the center of the history
+  const uint8_t mask = 32-1;
+  /* Hilbert coeffecients with a hamming window
+   * Values are 2/pi, 2/3pi, 2/7pi, 2/9pi, etc
+   * Stored in 15.16 fixed-point
+   */
+  const int16_t coef[] = {41353, 12829, 6638, 3753, 2087, 1079, 506, 247};
+  int32_t sum;
+  sum  = (src[(i- 1)&mask]-src[(i+ 1)&mask]) * (int32_t)coef[0];
+  sum += (src[(i- 3)&mask]-src[(i+ 3)&mask]) * (int32_t)coef[1];
+  sum += (src[(i- 5)&mask]-src[(i+ 5)&mask]) * (int32_t)coef[2];
+  sum += (src[(i- 7)&mask]-src[(i+ 7)&mask]) * (int32_t)coef[3];
+  sum += (src[(i- 9)&mask]-src[(i+ 9)&mask]) * (int32_t)coef[4];
+  sum += (src[(i-11)&mask]-src[(i+11)&mask]) * (int32_t)coef[5];
+  sum += (src[(i-13)&mask]-src[(i+13)&mask]) * (int32_t)coef[6];
+  sum += (src[(i-15)&mask]-src[(i+15)&mask]) * (int32_t)coef[7];
+  return sum;
 }
diff --git a/code/radio/hilbert.h b/code/radio/hilbert.h
index 7dd5e73..40ab6cc 100644
--- a/code/radio/hilbert.h
+++ b/code/radio/hilbert.h
@@ -6,4 +6,12 @@
  *
  * Generated with hamming window
  */
-int16_t hilbert19(int16_t * src);
+int32_t hilbert19(int16_t * src);
+
+/** 31-element Hilbert transform
+ *
+ * This implementation assumes a 32-element buffer, and wraps accordingly
+ * @param src The source data. Make sure I and Q are not interlieved
+ * @param i The index of the latest element. Older elements are assumed to have decreasing index.
+ */
+int32_t hilbert32(int16_t * src, uint8_t i);
diff --git a/code/radio/rx.c b/code/radio/rx.c
index a820293..99842af 100644
--- a/code/radio/rx.c
+++ b/code/radio/rx.c
@@ -7,8 +7,8 @@
 
 #include "rx.h"
 #include "ssb.h"
-#include "../drivers/speaker.h"
 #include "../drivers/si5351.h"
+#include "../drivers/speaker.h"
 
 /** Mailbox for received data */
 mailbox_t new_sample;
@@ -18,23 +18,16 @@ struct{
   enum radio_mode mode;
 }rx_cfg;
 
-THD_WORKING_AREA(waradio_rx, 128);
+THD_WORKING_AREA(waradio_rx, 512);
 THD_FUNCTION(radio_rx, arg){
   (void)arg;
-  const int len = 256;
-  int16_t * data = chCoreAllocFromBase(len*sizeof(int16_t), sizeof(int16_t), 0);
   while(1){
-    union complex c;
-    for( int i=0; i<len;){
-      chMBFetchTimeout(&new_sample, &c.msg, TIME_INFINITE);
-      data[i++] = c.real;
-      data[i++] = c.imag;
-    }
-    /** Process the received data */
-    int16_t out[len];
-    ssb_rx(out, data, len);
-    /** Fill buffer for audio out */
-    speakerUpdate(out, len);
+    if((USB==rx_cfg.mode) || (LSB==rx_cfg.mode))
+      ssb_rx(&new_sample, &(rx_cfg.mode));
+    else if(CW==rx_cfg.mode) // Currently use SSB for CW decoding
+      ssb_rx(&new_sample, &(rx_cfg.mode));
+    else
+      chThdSleepMilliseconds(50); // Don't hog all the CPU
   }
 }
 
@@ -46,7 +39,7 @@ THD_FUNCTION(radio_rx, arg){
  * We use a second-order IIR filter to prevent aliasing, followed
  * by an accumulate for decimation
  */
-static void adccallback(ADCDriver *adcp) {
+void adccallback(ADCDriver *adcp) {
   adcsample_t * ptr = adcp->samples;
   size_t len = adcp->depth/2; // This also determines downsample ratio
   // These are persistent variables for filter history
@@ -77,7 +70,7 @@ static void adccallback(ADCDriver *adcp) {
   chMBPostTimeout(&new_sample, c.msg, TIME_IMMEDIATE);
 }
 
-static void adcerrorcallback(ADCDriver *adcp, adcerror_t err) {
+void adcerrorcallback(ADCDriver *adcp, adcerror_t err) {
 
   (void)adcp;
   (void)err;
@@ -144,6 +137,7 @@ void rxStart(enum radio_mode mode, float frequency){
 }
 
 void rxStop(void){
+  rx_cfg.mode = STOPPED;
   adcStop(&ADCD1);
   speakerStop();
 }
diff --git a/code/radio/rx.h b/code/radio/rx.h
index 2c6431b..893ecce 100644
--- a/code/radio/rx.h
+++ b/code/radio/rx.h
@@ -16,6 +16,7 @@
  * This list will be expanded as more modes are supported
  */
 enum radio_mode {
+  STOPPED, //< Receiver is stopped
   CW, //< Continuous wave, also known as morse code
   USB,//< Upper Sideband, for frequencies above 10MHz
   LSB,//< Lower sideband, for frequencies below 10MHz
@@ -60,7 +61,7 @@ THD_FUNCTION(radio_rx, arg);
  * Performs initial low-pass filter and downsamples to 5kHz
  * Passes data to the Radio RX thread
  */
-static void adccallback(ADCDriver *adcp);
+void adccallback(ADCDriver *adcp);
 
 /** ADC init
  *
diff --git a/code/radio/ssb.c b/code/radio/ssb.c
index a9e220e..b68332f 100644
--- a/code/radio/ssb.c
+++ b/code/radio/ssb.c
@@ -5,15 +5,52 @@
  *      Author: marshal
  */
 #include "ssb.h"
+#include "hilbert.h"
+#include "../drivers/speaker.h"
 
-void ssb_rx(int16_t * dest, int16_t * src, size_t qty){
-  // Source data is interlieved In-phase / Quadrature phase
-  // Destination is single-channel
-  for(int i = 0; i < qty; ++i){
-    *dest++ = *src++ + *src++; // Return the sum of I and Q
+msg_t ssb_rx(mailbox_t * inbox, enum radio_mode * mode){
+  union complex c;
+  const int len = 32;
+  int16_t real[len];
+  int16_t imag[len];
+  while((USB==*mode) || (LSB==*mode)){
+    for( int i=0; i<len;){
+      /** Fetch new data */
+      msg_t m = chMBFetchTimeout(inbox, &c.msg, TIME_MS2I(10));
+      if(MSG_OK != m)
+        return m;
+      real[i] = c.real;
+      imag[i] = c.imag;
+      if(LSB==*mode)
+        imag[i]=-imag[i];
+      /** Process the received data */
+      int16_t out;
+      uint8_t j = (i-16)&31; // get to the center of the data
+#ifdef HIGH_EMPH // Remove low frequencies
+      int32_t h = hilbert32(imag, i);
+      out = real[j] - (h>>16);
+#else // Keep low frequencies
+      int32_t h = hilbert32(real, i);
+      out = imag[j] + (h>>16);
+#endif
+      /** Fill buffer for audio out
+       * We could optionally feed more than one sample in at a time*/
+      speakerUpdate(&out, 1);
+    }
   }
+  return MSG_OK;
 }
 
+const int32_t fscale = 1; //FIXME
+
 void ssb_tx(int16_t * amp, int32_t * freq, int16_t * src, size_t qty){
   // Source data is single-channel audio
+  int32_t f = freq[0];
+  for(size_t i = 0; i < (qty-18); ++i){
+    amp[i]=src[i+9];
+    // FIXME: Subtraction is not a differentiator
+    // Maybe the differentiation can be combined with the hilbert?
+    f -= hilbert19(src+i) * fscale;
+    freq[i]=f;
+  }
 }
diff --git a/code/radio/ssb.h b/code/radio/ssb.h
index fe0083b..bc836b5 100644
--- a/code/radio/ssb.h
+++ b/code/radio/ssb.h
@@ -6,11 +6,12 @@
  */
 #include <stdint.h>
 #include <stddef.h>
+#include "rx.h"
 
 /** Single sideband decoder
  *
  */
-void ssb_rx(int16_t * dest, int16_t * src, size_t qty);
+msg_t ssb_rx(mailbox_t * inbox, enum radio_mode * mode);
 
 /** Singgle sideband encoder
  *