diff --git a/firmware/include/si5351.h b/firmware/include/si5351.h
index 6a0eb1a..4f8b796 100644
--- a/firmware/include/si5351.h
+++ b/firmware/include/si5351.h
@@ -7,20 +7,25 @@
 
 static const uint8_t SI5351_ADDRESS = 96;
 
-enum si5351_param_base {
-    PLL_A = 26,
-    PLL_B = 34,
-    MULTISYNTH_0 = 42,
-    MULTOSYNTH_1 = 50,
-    MULTISYNTH_2 = 58
+enum si5351_pll {
+    SI5351_PLLA = 0,
+    SI5351_PLLB = 1,
 };
 
+static const uint8_t SI5351_PLL_BASE_ADDR[] = {26, 34};
+
+enum si5351_multisynth {
+    SI5351_MS0 = 0,
+    SI5351_MS1 = 1,
+    SI5351_MS2 = 2,
+};
+
+static const uint8_t SI5351_MULTISYNTH_BASE[] = {42, 50, 58};
+static const uint8_t SI5351_CLK_CTRL[] = {16, 17, 18};
+
 enum si5351_reg {
     CLK_ENABLE_CONTROL = 3,
     PLLX_SRC = 15,
-    CLK0_CONTROL = 16,
-    CLK1_CONTROL = 17,
-    CLK2_CONTROL = 18,
     PLL_RESET = 177,
     XTAL_LOAD_CAP = 183
 };
@@ -33,8 +38,11 @@ static inline uint8_t si5351_write8(uint8_t reg, uint8_t value) {
 }
 
 void si5351_init(uint32_t freq_xtal, uint32_t freq_a, uint32_t freq_b);
-void si5351_set_freq(enum si5351_param_base base, uint32_t freq);
-uint8_t si5351_write_params(enum si5351_param_base base, uint32_t p1, uint32_t p2, uint32_t p3);
+void si5351_ms_enable_output(enum si5351_multisynth synth);
+void si5351_ms_disable_output(enum si5351_multisynth synth);
+void si5351_ms_set_source(enum si5351_multisynth synth, enum si5351_pll pll);
+void si5351_ms_set_freq(enum si5351_multisynth synth, uint32_t freq);
+uint8_t si5351_write_params(uint8_t base, uint32_t p1, uint32_t p2, uint32_t p3);
 
 
 
diff --git a/firmware/main.c b/firmware/main.c
index 51193f8..9d1d016 100644
--- a/firmware/main.c
+++ b/firmware/main.c
@@ -8,8 +8,10 @@ int main(void) {
 
     twi_init();
 
-    si5351_init(25000000, 400000000, 400000000);
-    si5351_set_freq(MULTISYNTH_0, 7165000);
+    si5351_init(25000000, 400000000, 300000000);
+    si5351_ms_set_source(SI5351_MS0, SI5351_PLLB);
+    si5351_ms_set_freq(SI5351_MS0, 7165000);
+    si5351_ms_enable_output(SI5351_MS0);
 
 
 
diff --git a/firmware/si5351.c b/firmware/si5351.c
index 3ce455f..ad5165d 100644
--- a/firmware/si5351.c
+++ b/firmware/si5351.c
@@ -1,62 +1,79 @@
 #include "si5351.h"
 
-uint32_t si5351_freq_a;
-uint32_t si5351_freq_b;
+uint32_t si5351_pll_freq[2];
+uint8_t si5351_multisynth_pll[3];
+uint8_t si5351_outputs;
 
 void si5351_init(uint32_t freq_xtal, uint32_t freq_a, uint32_t freq_b) {
-    si5351_write8(CLK_ENABLE_CONTROL, 0x00); // Enable all outputs
+    si5351_pll_freq[SI5351_PLLA] = freq_a;
+    si5351_pll_freq[SI5351_PLLB] = freq_b;
+
+    si5351_outputs = 0xFF;
+    si5351_write8(CLK_ENABLE_CONTROL, si5351_outputs); // Disable all outputs
     si5351_write8(XTAL_LOAD_CAP, 0xD2);     //crystal load capacitor = 10pF
 
-    si5351_write8(CLK0_CONTROL, 0x0F);      // PLLA to CLK0, 8 mA output
     si5351_write8(PLL_RESET, 0xA0);         // Reset both PLLs
 
-    si5351_freq_a = freq_a;
-    si5351_freq_b = freq_b;
+    for(uint8_t i = 0; i < 3; i++) {
+        si5351_multisynth_pll[i] = 0; // == SI5351_PLLA
+        si5351_write8(SI5351_CLK_CTRL[i], 0x0F);  // MSi as Source, PLLA to MSi, 8 mA output
+    }
 
-    double fdiv = (double) si5351_freq_a / (double) freq_xtal;
+    for(uint8_t i = 0; i < 2; i++) {
+        double fdiv = (double) si5351_pll_freq[i] / (double) freq_xtal;
+
+        //TODO: Find better way to determine c and b
+        uint32_t c = 0x0FFFFF;
+        uint32_t a = (uint32_t) fdiv;
+        //TODO: Use modulo
+        double rm = fdiv - a;
+        uint32_t b = rm * c;
+        uint32_t p1  = 128 * a + (128 * b / c) - 512;
+        uint32_t p2 = 128 * b - c * (128 * b / c);
+        uint32_t p3 = c;
+        si5351_write_params(SI5351_PLL_BASE_ADDR[i], p1, p2, p3);
+    }
+}
+
+void si5351_ms_enable_output(enum si5351_multisynth synth) {
+    si5351_outputs &= ~(1 << synth);
+    si5351_write8(CLK_ENABLE_CONTROL, si5351_outputs);
+}
+
+void si5351_ms_disable_output(enum si5351_multisynth synth) {
+    si5351_outputs |= (1 << synth);
+    si5351_write8(CLK_ENABLE_CONTROL, si5351_outputs);
+}
+
+void si5351_ms_set_source(enum si5351_multisynth synth, enum si5351_pll pll) {
+    uint8_t value = 0x0F;  // MS as Source, PLLA to MS, 8 mA output
+    si5351_multisynth_pll[synth] = 0; // == PLLA
+
+    if(pll == SI5351_PLLB) {
+        value |= (1 << 5); // Bit 5 set PLLB to MS
+        si5351_multisynth_pll[synth] = 1; // == PLLB
+    }
+    si5351_write8(SI5351_CLK_CTRL[synth], value);
+}
+
+void si5351_ms_set_freq(enum si5351_multisynth synth, uint32_t freq) {
+    double fdiv = (double) si5351_pll_freq[si5351_multisynth_pll[synth]] / (double) freq;
 
     //TODO: Find better way to determine c and b
     uint32_t c = 0x0FFFFF;
     uint32_t a = (uint32_t) fdiv;
+    //TODO: Use modulo
     double rm = fdiv - a;
     uint32_t b = rm * c;
     uint32_t p1  = 128 * a + (128 * b / c) - 512;
     uint32_t p2 = 128 * b - c * (128 * b / c);
     uint32_t p3 = c;
-    si5351_write_params(PLL_A, p1, p2, p3);
-
-
-    fdiv = (double) freq_xtal / (double) si5351_freq_a;
-
-    //TODO: Find better way to determine c and b
-    c = 0x0FFFFF;
-    a = (uint32_t) fdiv;
-    rm = fdiv - a;
-    b = rm * c;
-    p1  = 128 * a + (128 * b / c) - 512;
-    p2 = 128 * b - c * (128 * b / c);
-    p3 = c;
-    si5351_write_params(PLL_B, p1, p2, p3);
-}
-
-void si5351_set_freq(enum si5351_param_base base, uint32_t freq) {
-    //TODO: Pick freq by clock source
-    double fdiv = (double) si5351_freq_a / (double) freq;
-
-    //TODO: Find better way to determine c and b
-    uint32_t c = 0x0FFFFF;
-    uint32_t a = (uint32_t) fdiv;
-    double rm = fdiv - a;
-    uint32_t b = rm * c;
-    uint32_t p1  = 128 * a + (128 * b / c) - 512;
-    uint32_t p2 = 128 * b - c * (128 * b / c);
-    uint32_t p3 = c;
-    si5351_write_params(base, p1, p2, p3);
+    si5351_write_params(SI5351_MULTISYNTH_BASE[synth], p1, p2, p3);
 }
 
 
 
-uint8_t si5351_write_params(enum si5351_param_base base, uint32_t p1, uint32_t p2, uint32_t p3) {
+uint8_t si5351_write_params(uint8_t base, uint32_t p1, uint32_t p2, uint32_t p3) {
     uint8_t data[9] = {
                     base,
                     (p3 & 0x00FF00) >> 8,