diff --git a/lib_xua/src/core/clocking/clockgen.xc b/lib_xua/src/core/clocking/clockgen.xc
index a489c047..efedac00 100644
--- a/lib_xua/src/core/clocking/clockgen.xc
+++ b/lib_xua/src/core/clocking/clockgen.xc
@@ -232,6 +232,12 @@ static inline int validSamples(Counter &counter, int clockIndex)
 #endif
 
 #if USE_SW_PLL
+/* Pointer to sw_pll struct to allow it to be used in separate SDM thread */ 
+unsafe 
+{
+    sw_pll_state_t * unsafe sw_pll_ptr = NULL;
+}
+
 unsigned InitSWPLL(sw_pll_state_t &sw_pll, unsigned mClk)
 {
     /* Autogenerated SDM App PLL setup by dco_model.py using 22.5792_1M profile */
@@ -294,30 +300,27 @@ unsigned InitSWPLL(sw_pll_state_t &sw_pll, unsigned mClk)
     return (XS1_TIMER_HZ / sw_pll_sdm_rate[clkIndex]);
 }
 
-void do_sw_pll_control( sw_pll_state_t sw_pll,
+void do_sw_pll_phase_frequency_detector( sw_pll_state_t sw_pll,
                         unsigned short mclk_time_stamp,
                         unsigned mclks_per_sample,
                         chanend c_sigma_delta,
                         int receivedSamples)
 {
+    /* Keep a store of the last mclk time stamp so we can work out the increment */
     static unsigned short last_mclk_time_stamp = 0;
-    static unsigned count = 0;
-    count++;
 
-    const unsigned expected_mclk_inc = mclks_per_sample * receivedSamples;
-    const unsigned short expected_mclk_count = (last_mclk_time_stamp + expected_mclk_inc) & 0xffff;
+    /* Calculate what the zero-error mclk count increment should be for this many samples */
+    const unsigned expected_mclk_inc = mclks_per_sample * receivedSamples / 2; /* divide by 2 because this fn is called per edge */
 
-    short f_error = (int)mclk_time_stamp - (int)expected_mclk_count;
+    /* Calculate actualy time-stamped mclk count increment is */
+    const unsigned short actual_mclk_inc = mclk_time_stamp - last_mclk_time_stamp;
 
+    /* The difference is the raw error in terms of mclk counts */
+    short f_error = (int)actual_mclk_inc - (int)expected_mclk_inc;
 
-    if(count == 30)
-    {
-        //calc mclk inc
-
-        outuint(c_sigma_delta, (unsigned) (1000000 + f_error));
-        count = 0;    
-    }
-
+    /* send PFD output to the sigma delta thread */
+    outuint(c_sigma_delta, (unsigned) (1000000 + f_error));
+ 
     last_mclk_time_stamp = mclk_time_stamp;
 }
 
@@ -326,9 +329,15 @@ void SigmaDeltaTask(chanend c_sigma_delta, unsigned sdm_interval){
        the first control value has been received. This avoids issues with 
        channel lockup if two tasks (eg. init and SDM) try to write at the same time. */ 
 
-    int dco_setting = 0;
-    sw_pll_sdm_state_t sdm_state;
-    sw_pll_init_sigma_delta(&sdm_state);
+    /* To be extra safe, spin on sw_pll_ptr until it has been initialised */
+    while(sw_pll_ptr == NULL);
+
+    int f_error = 0;
+    int dco_setting = SW_PLL_SDM_CTRL_MID_24; // TODO FIX ME
+    unsafe
+    {
+        sw_pll_init_sigma_delta(&sw_pll_ptr->sdm_state);
+    }
 
     tileref_t this_tile = get_local_tile_id();
 
@@ -344,8 +353,13 @@ void SigmaDeltaTask(chanend c_sigma_delta, unsigned sdm_interval){
         select
         {
             case inuint_byref(c_sigma_delta, tmp):
-                dco_setting = (int32_t)tmp;
-                printstr("sigma-delta got: "); printintln(dco_setting);
+                f_error = (int32_t)tmp;
+                printstr("sigma-delta got: "); printintln(f_error);
+                unsafe
+                {
+                    sw_pll_sdm_do_control_from_error(sw_pll_ptr, -f_error + 1000000);
+                    dco_setting = sw_pll_ptr->sdm_state.current_ctrl_val;
+                }
             break;
 
             // Do nothing & fall-through
@@ -367,9 +381,9 @@ void SigmaDeltaTask(chanend c_sigma_delta, unsigned sdm_interval){
            control value. This will avoid the writing of the
            frac reg from two different threads which may cause
            a channel deadlock. */
-        if(dco_setting != 0)
-        {
-            sw_pll_do_sigma_delta(&sdm_state, this_tile, dco_setting);
+        if(f_error != 0)
+        unsafe {
+            sw_pll_do_sigma_delta(&sw_pll_ptr->sdm_state, this_tile, dco_setting);
             send_ack_once = 1;
         }
         else if(send_ack_once)
@@ -524,6 +538,13 @@ void clockGen ( streaming chanend ?c_spdif_rx,
 #if USE_SW_PLL
     chan c_sigma_delta;
     sw_pll_state_t sw_pll;
+
+    /* Initialise before we par off the SDM task */
+    unsafe
+    {
+        sw_pll_ptr = &sw_pll;
+    }
+
     unsigned sdm_interval = InitSWPLL(sw_pll, MCLK_48);
 
     par
@@ -708,7 +729,7 @@ void clockGen ( streaming chanend ?c_spdif_rx,
                     printstr("c_mclk_change: "); printuintln(selected_mclk_rate);
                     disable_sigma_delta(c_sigma_delta); /* Blocks until SDM is idle */
                     InitSWPLL(sw_pll, selected_mclk_rate);
-                    outuint(c_sigma_delta, sw_pll.sdm_state.ctrl_mid_point); /* Send ctrl mid point to enable SDM */
+                    outuint(c_sigma_delta, sw_pll.sdm_state.ctrl_mid_point); /* Send ctrl mid point to re-enable SDM and set to nominal mclk */
                     printstr("swpll int'd\n");
 #endif
                     break;
@@ -792,7 +813,7 @@ void clockGen ( streaming chanend ?c_spdif_rx,
                                 timeNextEdge = spdifRxTime + LOCAL_CLOCK_INCREMENT + LOCAL_CLOCK_MARGIN;
 
 #if USE_SW_PLL
-                                do_sw_pll_control(sw_pll, mclk_time_stamp, mclks_per_sample, c_sigma_delta, spdifCounters.receivedSamples);
+                                do_sw_pll_phase_frequency_detector(sw_pll, mclk_time_stamp, mclks_per_sample, c_sigma_delta, spdifCounters.receivedSamples);
 #else
                                 /* Toggle edge */
                                 i_pll_ref.toggle_timed(1);
@@ -902,7 +923,7 @@ void clockGen ( streaming chanend ?c_spdif_rx,
                                                 timeNextEdge = adatReceivedTime + LOCAL_CLOCK_INCREMENT + LOCAL_CLOCK_MARGIN;
 
 #if USE_SW_PLL
-                                                do_sw_pll_control(sw_pll, mclk_time_stamp, mclks_per_sample, c_sigma_delta, adatCounters.receivedSamples);
+                                                do_sw_pll_phase_frequency_detector(sw_pll, mclk_time_stamp, mclks_per_sample, c_sigma_delta, adatCounters.receivedSamples);
 #else
                                                 /* Toggle edge */
                                                 i_pll_ref.toggle_timed(1);