diff --git a/lib_xua/src/core/buffer/ep/ep_buffer.xc b/lib_xua/src/core/buffer/ep/ep_buffer.xc index 050ce5b5f..f844d3a4d 100644 --- a/lib_xua/src/core/buffer/ep/ep_buffer.xc +++ b/lib_xua/src/core/buffer/ep/ep_buffer.xc @@ -385,8 +385,11 @@ void XUA_Buffer_Ep(register chanend c_aud_out, pfd_ppm_max); outuint(c_sw_pll, masterClockFreq); outct(c_sw_pll, XS1_CT_END); - inuint(c_sw_pll); /* receive ACK */ - inct(c_sw_pll); + + stability_state_t stability_state; + init_stability(stability_state, 5, 10); /* Typically runs at -2 to +2 maximum when locked so 5 or less is good. + 10 = number of error calcs at below threshold before considered stable */ + int audio_needs_ack = 0; /* flag saying audio needs ack to release following PLL change */ #else /* XUA_USE_SW_PLL */ timer t_sofCheck; @@ -593,6 +596,15 @@ void XUA_Buffer_Ep(register chanend c_aud_out, outuint(c_sw_pll, error); outct(c_sw_pll, XS1_CT_END); + printintln(error); + + if((check_stability(stability_state, error) == PLL_STABLE) && audio_needs_ack) + { + c_audio_rate_change <: 0; /* ACK back to audio to release to start */ + audio_needs_ack = 0; + printstr("stable\n"); + } + #else /* (XUA_USE_SW_PLL) */ /* Do toggle for CS2100 reference clock */ /* Port is accessed via interface to allow flexibilty with location */ @@ -1040,19 +1052,13 @@ void XUA_Buffer_Ep(register chanend c_aud_out, pfd_ppm_max); restart_sigma_delta(c_sw_pll, selected_mclk_rate); /* Delay ACK until sw_pll says it is ready */ + reset_stability(stability_state); + audio_needs_ack = 1; #else c_audio_rate_change <: 0; /* ACK back to audio to release I2S immediately */ #endif /* XUA_USE_SW_PLL */ break; -#if (XUA_USE_SW_PLL) - /* This is fired when sw_pll has completed initialising a new mclk_rate */ - case inuint_byref(c_sw_pll, u_tmp): - inct(c_sw_pll); - c_audio_rate_change <: 0; /* ACK back to audio to release */ - - break; -#endif /* (XUA_USE_SW_PLL) */ #endif /* (XUA_SYNCMODE == XUA_SYNCMODE_SYNC) */ #ifdef IAP diff --git a/lib_xua/src/core/clocking/clockgen.xc b/lib_xua/src/core/clocking/clockgen.xc index f44c7f37a..06a1479eb 100644 --- a/lib_xua/src/core/clocking/clockgen.xc +++ b/lib_xua/src/core/clocking/clockgen.xc @@ -348,6 +348,10 @@ void clockGen ( streaming chanend ?c_spdif_rx, #if ((XUA_SPDIF_RX_EN || XUA_ADAT_RX_EN) && XUA_USE_SW_PLL) int reset_sw_pll_pfd = 1; int require_ack_to_audio = 0; + stability_state_t stability_state; + init_stability(stability_state, 5, 10); /* Typically runs at -1 to +1 maximum when locked so 5 or less is good. + 10 = number of error calcs at below threshold before considered stable */ + restart_sigma_delta(c_sw_pll, MCLK_48); /* default to 48kHz - this will be reset shortly when host selects rate */ #endif @@ -469,7 +473,13 @@ void clockGen ( streaming chanend ?c_spdif_rx, case t_local when timerafter(timeNextEdge) :> void: #if XUA_USE_SW_PLL - /* Do nothing - hold the most recent sw_pll setting */ + /* Hold the most recent sw_pll setting */ + if(require_ack_to_audio) + { + c_audio_rate_change <: 0; /* ACK back to audio to release to start I2S */ + require_ack_to_audio = 0; + printstr("stable int\n"); + } #else /* Setup next local clock edge */ i_pll_ref.toggle_timed(0); @@ -519,19 +529,6 @@ void clockGen ( streaming chanend ?c_spdif_rx, break; #endif -#if ((XUA_SPDIF_RX_EN || XUA_ADAT_RX_EN) && XUA_USE_SW_PLL) - case inuint_byref(c_sw_pll, tmp): - inct(c_sw_pll); - /* Send ACK back to audiohub to allow I2S to start - This happens only on SDM restart and only once */ - if(require_ack_to_audio) - { - c_audio_rate_change <: tmp; - require_ack_to_audio = 0; - } - break; -#endif - #if (XUA_SPDIF_RX_EN || XUA_ADAT_RX_EN) /* Receive notification of audio streaming settings change and store */ case c_audio_rate_change :> selected_mclk_rate: @@ -540,8 +537,10 @@ void clockGen ( streaming chanend ?c_spdif_rx, mclks_per_sample = selected_mclk_rate / selected_sample_rate; restart_sigma_delta(c_sw_pll, selected_mclk_rate); reset_sw_pll_pfd = 1; + reset_stability(stability_state); /* We will shedule an ACK when sigma delta is up and running */ require_ack_to_audio = 1; + printstr("SR\n"); #else /* Send ACK immediately as we are good to go if not using SW_PLL */ c_audio_rate_change <: 0; @@ -627,11 +626,18 @@ void clockGen ( streaming chanend ?c_spdif_rx, timeNextEdge = spdifRxTime + LOCAL_CLOCK_INCREMENT + LOCAL_CLOCK_MARGIN; #if XUA_USE_SW_PLL - do_sw_pll_phase_frequency_detector_dig_rx( mclk_time_stamp, - mclks_per_sample, - c_sw_pll, - spdifCounters.receivedSamples, - reset_sw_pll_pfd); + int stable = do_sw_pll_phase_frequency_detector_dig_rx( mclk_time_stamp, + mclks_per_sample, + c_sw_pll, + spdifCounters.receivedSamples, + reset_sw_pll_pfd, + stability_state); + if(stable && require_ack_to_audio) + { + c_audio_rate_change <: 0; /* ACK back to audio to release to start */ + require_ack_to_audio = 0; + printstr("stable ext\n"); + } #else /* Toggle edge */ i_pll_ref.toggle_timed(1); @@ -749,6 +755,13 @@ void clockGen ( streaming chanend ?c_spdif_rx, c_sw_pll, adatCounters.receivedSamples, reset_sw_pll_pfd); + + if(stable && require_ack_to_audio) + { + c_audio_rate_change <: 0; /* ACK back to audio to release to start */ + require_ack_to_audio = 0; + printstr("stable ext\n"); + } #else /* Toggle edge */ i_pll_ref.toggle_timed(1); diff --git a/lib_xua/src/core/clocking/sw_pll_wrapper.h b/lib_xua/src/core/clocking/sw_pll_wrapper.h index edc86096f..0b57d8319 100644 --- a/lib_xua/src/core/clocking/sw_pll_wrapper.h +++ b/lib_xua/src/core/clocking/sw_pll_wrapper.h @@ -16,6 +16,21 @@ extern "C" #define DISABLE_SDM 0x10000000 +typedef enum stability_status_t +{ + PLL_UNSTABLE, + PLL_STABLE +} stability_status_t; + +typedef struct stability_state_t +{ + int stabilty_error_threshold; /* Magnitude of error below which we consider stability achieved */ + int stabilty_count_threshold; /* Number of PFD calculations at below threshold before stable */ + int instablity_counter; /* 0 = stable else unstable */ + +} stability_state_t; + + /** Task that receives an error term, passes it through a PI controller and periodically * calclulates a sigma delta output value and sends it to the PLL fractional register. * @@ -33,17 +48,20 @@ void restart_sigma_delta(chanend c_sw_pll, unsigned mclk_rate); /** Performs a frequency comparsion between the incoming digital Rx stream and the local mclk. * - * \param mclk_time_stamp The captured mclk count (using port timer) at the time of sample Rx. - * \param mclks_per_sample The nominal number of mclks per audio sample. - * \param c_sw_pll Channel connected to the sigma delta and controller thread. - * \param receivedSamples The number of received samples since tha last call to this function. - * \param reset_sw_pll_pfd Reference to a flag which will be used to signal reset of this function's state. + * \param mclk_time_stamp The captured mclk count (using port timer) at the time of sample Rx. + * \param mclks_per_sample The nominal number of mclks per audio sample. + * \param c_sw_pll Channel connected to the sigma delta and controller thread. + * \param receivedSamples The number of received samples since tha last call to this function. + * \param reset_sw_pll_pfd Reference to a flag which will be used to signal reset of this function's state. + * + * \return the calculated frequency error */ -void do_sw_pll_phase_frequency_detector_dig_rx( unsigned short mclk_time_stamp, +int do_sw_pll_phase_frequency_detector_dig_rx( unsigned short mclk_time_stamp, unsigned mclks_per_sample, chanend c_sw_pll, int receivedSamples, - int &reset_sw_pll_pfd); + int &reset_sw_pll_pfd, + stability_state_t &stability_state); /** Initilaises the software PLL both hardware and state. Sets the mclk frequency to a nominal point. * @@ -53,5 +71,11 @@ void do_sw_pll_phase_frequency_detector_dig_rx( unsigned short mclk_time_stamp, * returns The SDM update interval in ticks and the initial DCO setting for nominal frequency */ {unsigned, unsigned} InitSWPLL(sw_pll_state_t &sw_pll, unsigned mClk); +void init_stability(stability_state_t &state, int stabilty_error_threshold, int stabilty_count_threshold); + +void reset_stability(stability_state_t &state); + +int check_stability(stability_state_t &state, int error); + #endif /* XUA_USE_SW_PLL */ #endif /* _SW_PLL_WRAPPPER_H_ */ diff --git a/lib_xua/src/core/clocking/sw_pll_wrapper.xc b/lib_xua/src/core/clocking/sw_pll_wrapper.xc index beb69ee8b..e4676c311 100644 --- a/lib_xua/src/core/clocking/sw_pll_wrapper.xc +++ b/lib_xua/src/core/clocking/sw_pll_wrapper.xc @@ -3,6 +3,7 @@ #include #include #include +#include #include "sw_pll_wrapper.h" #include "xua.h" @@ -72,13 +73,15 @@ return {XS1_TIMER_HZ / sw_pll_sdm_rate[clkIndex], sw_pll_sdm_ctrl_mid[clkIndex]}; } -void do_sw_pll_phase_frequency_detector_dig_rx( unsigned short mclk_time_stamp, +int do_sw_pll_phase_frequency_detector_dig_rx( unsigned short mclk_time_stamp, unsigned mclks_per_sample, chanend c_sw_pll, int receivedSamples, - int &reset_sw_pll_pfd) + int &reset_sw_pll_pfd, + stability_state_t &stability_state) { const unsigned control_loop_rate_divider = 6; /* 300Hz * 2 edges / 6 -> 100Hz loop rate */ + static int pll_stable = PLL_UNSTABLE; static unsigned control_loop_counter = 0; static unsigned total_received_samples = 0; @@ -108,11 +111,17 @@ void do_sw_pll_phase_frequency_detector_dig_rx( unsigned short mclk_time_stamp, /* send PFD output to the sigma delta thread */ outuint(c_sw_pll, (int) f_error); outct(c_sw_pll, XS1_CT_END); - + + printintln(f_error); + pll_stable = check_stability(stability_state, f_error); + pll_stable = PLL_STABLE; // TMP - because host cycles through SRs, it never reaches stability + last_mclk_time_stamp = mclk_time_stamp; control_loop_counter = 0; total_received_samples = 0; } + + return pll_stable; } void sw_pll_task(chanend c_sw_pll){ @@ -141,9 +150,6 @@ void sw_pll_task(chanend c_sw_pll){ time_trigger += sdm_interval; /* ensure first loop has correct delay */ int running = 1; - outuint(c_sw_pll, 0); /* Signal back via clockgen to audio to start I2S */ - outct(c_sw_pll, XS1_CT_END); - unsigned rx_word = 0; while(running) { @@ -199,4 +205,39 @@ void restart_sigma_delta(chanend c_sw_pll, unsigned selected_mclk_rate) outct(c_sw_pll, XS1_CT_END); } + +void init_stability(stability_state_t &state, int stabilty_error_threshold, int stabilty_count_threshold) +{ + state.stabilty_error_threshold = stabilty_error_threshold; + state.stabilty_count_threshold = stabilty_count_threshold; + state.instablity_counter = state.stabilty_count_threshold; +} + +void reset_stability(stability_state_t &state) +{ + state.instablity_counter = state.stabilty_count_threshold; +} + +int check_stability(stability_state_t &state, int error) +{ + if(abs(error) < state.stabilty_error_threshold) + { + if(state.instablity_counter > 0) + { + state.instablity_counter--; + } + else + { + return PLL_STABLE; + } + } + else + { + /* reset stability */ + state.instablity_counter = state.stabilty_count_threshold; + } + + return PLL_UNSTABLE; +} + #endif /* XUA_USE_SW_PLL */