diff --git a/dsp.c b/dsp.c
index a515a8a..d8fde59 100644
--- a/dsp.c
+++ b/dsp.c
@@ -41,7 +41,6 @@ hilbert_transform(void)
     int32_t accn0 = 0;
     int32_t acc1 = 0;
     int32_t accn1 = 0;
-    int32_t s;
 
     for (i = 0; i < 8; i += 2) {
       uint32_t c = *(uint32_t*)&hilbert31_coeffs[i];
@@ -75,6 +74,7 @@ hilbert_transform(void)
 }
 
 void calclate_gamma(void)
+#if 0
 {
   __SIMD32_TYPE *r = __SIMD32_CONST(refiq_buf);
   __SIMD32_TYPE *s = __SIMD32_CONST(samp_buf);
@@ -98,7 +98,30 @@ void calclate_gamma(void)
   gamma_real = acc_r / 65536;
   gamma_imag = acc_i / 65536;
 }
+#else
+{
+  int16_t *r = refiq_buf;
+  int16_t *s = samp_buf;
+  int len = SAMPLE_LEN/5;
+  double acc_r = 0;
+  double acc_i = 0;
+  double acc_ref = 0;
+  int i;
+  double rn;
 
+  for (i = 0; i < len; i++) {
+    int16_t s0 = *s++;
+    int16_t rr = *r++;
+    int16_t ri = *r++;
+    acc_r += (double)(s0 * rr);
+    acc_i += (double)(s0 * ri);
+    acc_ref += (double)rr*rr + (double)ri*ri;
+  }
+  rn = sqrtf(acc_ref / len);
+  gamma_real = 16 * acc_r / rn / len;
+  gamma_imag = 16 * acc_i / rn / len;
+}
+#endif
 
 void
 dsp_process(int16_t *capture, size_t length)
diff --git a/main.c b/main.c
index 5d13ea2..99459cf 100644
--- a/main.c
+++ b/main.c
@@ -13,7 +13,8 @@ RTCDateTime timespec;
 
 
 static const I2CConfig i2ccfg = {
-  0x00902025, //voodoo magic
+  0x00300506, //voodoo magic 400kHz @ HSI 8MHz
+  //0x00902025, //voodoo magic
   //0x00420F13,  // 100kHz @ 72MHz
   0,
   0
@@ -143,14 +144,7 @@ static void cmd_dac(BaseSequentialStream *chp, int argc, char *argv[])
         return;
     }
     value = atoi(argv[0]);
-#if 1
     dacPutChannelX(&DACD2, 0, value);
-#else
-    if (value & 1)
-      palSetPad(GPIOA, 5);
-    else
-      palClearPad(GPIOA, 5);
-#endif
 }
 
 
@@ -187,7 +181,7 @@ void i2s_end_callback(I2SDriver *i2sp, size_t offset, size_t n)
   int16_t *p = &rx_buffer[offset];
   (void)i2sp;
   (void)n;
-  palClearPad(GPIOC, GPIOC_LED);
+  //palClearPad(GPIOC, GPIOC_LED);
 
   if (!dsp_disabled)
     dsp_process(p, n);
@@ -219,7 +213,7 @@ void i2s_end_callback(I2SDriver *i2sp, size_t offset, size_t n)
   stat.last_counter_value = cnt_s;
 #endif
   stat.callback_count++;
-  palSetPad(GPIOC, GPIOC_LED);
+  //palSetPad(GPIOC, GPIOC_LED);
 }
 
 static const I2SConfig i2sconfig = {
@@ -261,7 +255,7 @@ static void cmd_gamma(BaseSequentialStream *chp, int argc, char *argv[])
   (void)argc;
   (void)argv;
 
-  wait_count = 3;
+  wait_count = 4;
   while (wait_count)
     ;
   dsp_disabled = TRUE;
@@ -271,6 +265,47 @@ static void cmd_gamma(BaseSequentialStream *chp, int argc, char *argv[])
   chprintf(chp, "%d %d\r\n", gamma_real, gamma_imag);
 }
 
+static void cmd_scan(BaseSequentialStream *chp, int argc, char *argv[])
+{
+  int i;
+  int len = 100;
+  (void)argc;
+  (void)argv;
+
+  for (i = 1; i <= len; i++) {
+    int32_t freq = i * 3000000L;
+    wait_count = 4;
+    while (wait_count)
+      ;
+    dsp_disabled = TRUE;
+    set_frequency(freq);
+    palClearPad(GPIOC, GPIOC_LED);
+    calclate_gamma();
+    palSetPad(GPIOC, GPIOC_LED);
+    dsp_disabled = FALSE;
+    chprintf(chp, "%d %d %d\r\n", freq, gamma_real, gamma_imag);
+  }
+}
+
+static void cmd_test(BaseSequentialStream *chp, int argc, char *argv[])
+{
+  int i;
+  (void)argc;
+  (void)argv;
+
+  for (i = 0; i < 100; i++) {
+    palClearPad(GPIOC, GPIOC_LED);
+    set_frequency(10000000);
+    palSetPad(GPIOC, GPIOC_LED);
+    chThdSleepMilliseconds(50);
+
+    palClearPad(GPIOC, GPIOC_LED);
+    set_frequency(100000000);
+    palSetPad(GPIOC, GPIOC_LED);
+    chThdSleepMilliseconds(50);
+  }
+}
+
 static void cmd_gain(BaseSequentialStream *chp, int argc, char *argv[])
 {
   int rvalue;
@@ -352,6 +387,8 @@ static const ShellCommand commands[] =
     { "gain", cmd_gain },
     { "power", cmd_power },
     { "gamma", cmd_gamma },
+    { "scan", cmd_scan },
+    { "test", cmd_test },
     { NULL, NULL }
 };
 
diff --git a/si5351.c b/si5351.c
index 96b5cd2..97708f0 100644
--- a/si5351.c
+++ b/si5351.c
@@ -53,10 +53,13 @@ si5351_init(void)
 
 void si5351_disable_output(void)
 {
+  uint8_t reg[4];
   si5351_write(SI5351_REG_3_OUTPUT_ENABLE_CONTROL, 0xff);
-  si5351_write(SI5351_REG_16_CLK0_CONTROL, 0x80);
-  si5351_write(SI5351_REG_17_CLK1_CONTROL, 0x80);
-  si5351_write(SI5351_REG_18_CLK2_CONTROL, 0x80);
+  reg[0] = SI5351_REG_16_CLK0_CONTROL;
+  reg[1] = SI5351_CLK_POWERDOWN;
+  reg[2] = SI5351_CLK_POWERDOWN;
+  reg[3] = SI5351_CLK_POWERDOWN;
+  si5351_bulk_write(reg, 4);
 }
 
 void si5351_enable_output(void)
@@ -80,8 +83,6 @@ void si5351_setupPLL(uint8_t pll, /* SI5351_PLL_A or SI5351_PLL_B */
     SI5351_REG_26_PLL_A,
     SI5351_REG_34_PLL_B
   };
-  uint8_t baseaddr = pllreg_base[pll];
-
   uint32_t P1;
   uint32_t P2;
   uint32_t P3;
@@ -115,14 +116,17 @@ void si5351_setupPLL(uint8_t pll, /* SI5351_PLL_A or SI5351_PLL_B */
   }
 
   /* The datasheet is a nightmare of typos and inconsistencies here! */
-  si5351_write(baseaddr,   (P3 & 0x0000FF00) >> 8);
-  si5351_write(baseaddr+1, (P3 & 0x000000FF));
-  si5351_write(baseaddr+2, (P1 & 0x00030000) >> 16);
-  si5351_write(baseaddr+3, (P1 & 0x0000FF00) >> 8);
-  si5351_write(baseaddr+4, (P1 & 0x000000FF));
-  si5351_write(baseaddr+5, ((P3 & 0x000F0000) >> 12) | ((P2 & 0x000F0000) >> 16) );
-  si5351_write(baseaddr+6, (P2 & 0x0000FF00) >> 8);
-  si5351_write(baseaddr+7, (P2 & 0x000000FF));
+  uint8_t reg[9];
+  reg[0] = pllreg_base[pll];
+  reg[1] = (P3 & 0x0000FF00) >> 8;
+  reg[2] = (P3 & 0x000000FF);
+  reg[3] = (P1 & 0x00030000) >> 16;
+  reg[4] = (P1 & 0x0000FF00) >> 8;
+  reg[5] = (P1 & 0x000000FF);
+  reg[6] = ((P3 & 0x000F0000) >> 12) | ((P2 & 0x000F0000) >> 16);
+  reg[7] = (P2 & 0x0000FF00) >> 8;
+  reg[8] = (P2 & 0x000000FF);
+  si5351_bulk_write(reg, 9);
 }
 
 void 
@@ -139,7 +143,6 @@ si5351_setupMultisynth(uint8_t     output,
     SI5351_REG_50_MULTISYNTH1,
     SI5351_REG_58_MULTISYNTH2,
   };
-  uint8_t baseaddr = msreg_base[output];
   const uint8_t clkctrl[] = {
     SI5351_REG_16_CLK0_CONTROL,
     SI5351_REG_17_CLK1_CONTROL,
@@ -179,14 +182,17 @@ si5351_setupMultisynth(uint8_t     output,
   }
 
   /* Set the MSx config registers */
-  si5351_write(baseaddr,   (P3 & 0x0000FF00) >> 8);
-  si5351_write(baseaddr+1, (P3 & 0x000000FF));
-  si5351_write(baseaddr+2, ((P1 & 0x00030000) >> 16) | div4);
-  si5351_write(baseaddr+3, (P1 & 0x0000FF00) >> 8);
-  si5351_write(baseaddr+4, (P1 & 0x000000FF));
-  si5351_write(baseaddr+5, ((P3 & 0x000F0000) >> 12) | ((P2 & 0x000F0000) >> 16));
-  si5351_write(baseaddr+6, (P2 & 0x0000FF00) >> 8);
-  si5351_write(baseaddr+7, (P2 & 0x000000FF));
+  uint8_t reg[9];
+  reg[0] = msreg_base[output];
+  reg[1] = (P3 & 0x0000FF00) >> 8;
+  reg[2] = (P3 & 0x000000FF);
+  reg[3] = ((P1 & 0x00030000) >> 16) | div4;
+  reg[4] = (P1 & 0x0000FF00) >> 8;
+  reg[5] = (P1 & 0x000000FF);
+  reg[6] = ((P3 & 0x000F0000) >> 12) | ((P2 & 0x000F0000) >> 16);
+  reg[7] = (P2 & 0x0000FF00) >> 8;
+  reg[8] = (P2 & 0x000000FF);
+  si5351_bulk_write(reg, 9);
 
   /* Configure the clk control and enable the output */
   dat = drive_strength | SI5351_CLK_INPUT_MULTISYNTH_N;