diff --git a/firmware/si5351.c b/firmware/si5351.c
index ad5165d..9e88960 100644
--- a/firmware/si5351.c
+++ b/firmware/si5351.c
@@ -20,14 +20,13 @@ void si5351_init(uint32_t freq_xtal, uint32_t freq_a, uint32_t freq_b) {
     }
 
     for(uint8_t i = 0; i < 2; i++) {
-        double fdiv = (double) si5351_pll_freq[i] / (double) freq_xtal;
+        uint32_t fdiv =  si5351_pll_freq[i] /  freq_xtal;
+        uint32_t rm =  si5351_pll_freq[i] %  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 b = (uint32_t) ((uint64_t) rm * (uint64_t) c / (uint64_t) freq_xtal);
         uint32_t p1  = 128 * a + (128 * b / c) - 512;
         uint32_t p2 = 128 * b - c * (128 * b / c);
         uint32_t p3 = c;
@@ -57,14 +56,13 @@ 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) {
-    double fdiv = (double) si5351_pll_freq[si5351_multisynth_pll[synth]] / (double) freq;
+    uint32_t fdiv = si5351_pll_freq[si5351_multisynth_pll[synth]] / freq;
+    uint32_t rm = si5351_pll_freq[si5351_multisynth_pll[synth]] % 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 a = fdiv;
+    uint32_t b = (uint32_t) ((uint64_t) rm * (uint64_t) c / (uint64_t) freq);
     uint32_t p1  = 128 * a + (128 * b / c) - 512;
     uint32_t p2 = 128 * b - c * (128 * b / c);
     uint32_t p3 = c;