diff --git a/lib_xua/src/core/clocking/clockgen.xc b/lib_xua/src/core/clocking/clockgen.xc index d70124fe..d67dcae1 100644 --- a/lib_xua/src/core/clocking/clockgen.xc +++ b/lib_xua/src/core/clocking/clockgen.xc @@ -236,6 +236,8 @@ unsafe sw_pll_state_t * unsafe sw_pll_ptr = NULL; } +#define DISABLE_SDM 0x1000000 /* Control value to disable SDM. Outside of normal range.*/ + unsigned InitSWPLL(sw_pll_state_t &sw_pll, unsigned mClk) { /* Autogenerated SDM App PLL setup by dco_model.py using 22.5792_1M profile */ @@ -292,6 +294,8 @@ unsigned InitSWPLL(sw_pll_state_t &sw_pll, unsigned mClk) sw_pll_sdm_ctrl_mid[clkIndex], 3000 /* PPM_RANGE (FOR PFD) Don't care for this API*/ ); + /* Reset SDM too */ + sw_pll_init_sigma_delta(&sw_pll.sdm_state); printstr("Init sw_pll: "); printuintln(mClk); @@ -302,7 +306,8 @@ 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) + int receivedSamples, + int &reset_sw_pll_pfd) { const unsigned control_loop_rate_divider = 6; /* 300Hz * 2 edges / 6 -> 100Hz loop rate */ static unsigned control_loop_counter = 0; @@ -325,9 +330,16 @@ void do_sw_pll_phase_frequency_detector( sw_pll_state_t sw_pll, /* The difference is the raw error in terms of mclk counts */ short f_error = (int)actual_mclk_inc - (int)expected_mclk_inc; + if(reset_sw_pll_pfd) + { + f_error = 0; /* Skip first measurement as it will likely be very out */ + reset_sw_pll_pfd = 0; + } + printintln(f_error); + /* send PFD output to the sigma delta thread */ - outuint(c_sigma_delta, (unsigned) (1000000 + f_error)); + outuint(c_sigma_delta, (int) f_error); last_mclk_time_stamp = mclk_time_stamp; control_loop_counter = 0; @@ -344,7 +356,7 @@ void SigmaDeltaTask(chanend c_sigma_delta, unsigned sdm_interval){ while(sw_pll_ptr == NULL); int f_error = 0; - int dco_setting = SW_PLL_SDM_CTRL_MID_24; // TODO FIX ME + int dco_setting = SW_PLL_SDM_CTRL_MID_24; // TODO Assume 24.576MHz? unsafe { sw_pll_init_sigma_delta(&sw_pll_ptr->sdm_state); @@ -364,11 +376,19 @@ void SigmaDeltaTask(chanend c_sigma_delta, unsigned sdm_interval){ select { case inuint_byref(c_sigma_delta, tmp): - f_error = (int32_t)tmp; - unsafe + if(tmp == DISABLE_SDM) { - sw_pll_sdm_do_control_from_error(sw_pll_ptr, -f_error + 1000000); - dco_setting = sw_pll_ptr->sdm_state.current_ctrl_val; + f_error = 0; + send_ack_once = 1; + } + else + { + f_error = (int32_t)tmp; + unsafe + { + sw_pll_sdm_do_control_from_error(sw_pll_ptr, -f_error); + dco_setting = sw_pll_ptr->sdm_state.current_ctrl_val; + } } break; @@ -391,7 +411,7 @@ 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(f_error != 0) + if(tmp != DISABLE_SDM) unsafe { sw_pll_do_sigma_delta(&sw_pll_ptr->sdm_state, this_tile, dco_setting); send_ack_once = 1; @@ -408,7 +428,7 @@ void SigmaDeltaTask(chanend c_sigma_delta, unsigned sdm_interval){ void disable_sigma_delta(chanend c_sigma_delta) { - outuint(c_sigma_delta, 0); /* Stops SD */ + outuint(c_sigma_delta, DISABLE_SDM); /* Stops SDM */ inuint(c_sigma_delta); /* Wait for ACK so we know reg write is complete */ } @@ -548,6 +568,7 @@ void clockGen ( streaming chanend ?c_spdif_rx, #if USE_SW_PLL chan c_sigma_delta; sw_pll_state_t sw_pll; + int reset_sw_pll_pfd = 1; /* Initialise before we par off the SDM thread/ We share the sw_pll struct across threads and this @@ -741,7 +762,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 re-enable SDM and set to nominal mclk */ + reset_sw_pll_pfd = 1; printstr("swpll int'd\n"); #endif break; @@ -825,7 +846,7 @@ void clockGen ( streaming chanend ?c_spdif_rx, timeNextEdge = spdifRxTime + LOCAL_CLOCK_INCREMENT + LOCAL_CLOCK_MARGIN; #if USE_SW_PLL - do_sw_pll_phase_frequency_detector(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, reset_sw_pll_pfd); #else /* Toggle edge */ i_pll_ref.toggle_timed(1); @@ -935,7 +956,7 @@ void clockGen ( streaming chanend ?c_spdif_rx, timeNextEdge = adatReceivedTime + LOCAL_CLOCK_INCREMENT + LOCAL_CLOCK_MARGIN; #if USE_SW_PLL - do_sw_pll_phase_frequency_detector(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, reset_sw_pll_pfd); #else /* Toggle edge */ i_pll_ref.toggle_timed(1); @@ -982,7 +1003,7 @@ void clockGen ( streaming chanend ?c_spdif_rx, { /* We're out of S/PDIF samples, mark underflow condition */ spdifUnderflow = 1; - printstr("spu-\n"); // DELME + printstr("spu+\n"); // DELME spdifLeft = 0; }