root/trunk/libffado/src/libieee1394/CycleTimerHelper.cpp

Revision 1016, 27.2 kB (checked in by ppalmers, 16 years ago)

- move lock to improve timing of ctr read. grabbing locks can take some time (esp. on non-rt kernels). we can read the data before locking anyway.
- add some debugging to track the quality of the CTR DLL updates
- relax the filter coeffs

Line 
1 /*
2  * Copyright (C) 2005-2008 by Pieter Palmers
3  *
4  * This file is part of FFADO
5  * FFADO = Free Firewire (pro-)audio drivers for linux
6  *
7  * FFADO is based upon FreeBoB
8  *
9  * This program is free software: you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation, either version 2 of the License, or
12  * (at your option) version 3 of the License.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
21  *
22  */
23
24 #include "config.h"
25
26 #include "CycleTimerHelper.h"
27 #include "ieee1394service.h"
28 #include "libutil/PosixThread.h"
29 #include "libutil/Atomic.h"
30 #include "libutil/Watchdog.h"
31
32 #define DLL_PI        (3.141592653589793238)
33 #define DLL_SQRT2     (1.414213562373095049)
34
35 // the high-bandwidth coefficients are used
36 // to speed up inital tracking
37 #define DLL_BANDWIDTH_HIGH (0.1)
38 #define DLL_OMEGA_HIGH     (2.0*DLL_PI*DLL_BANDWIDTH_HIGH)
39 #define DLL_COEFF_B_HIGH   (DLL_SQRT2 * DLL_OMEGA_HIGH)
40 #define DLL_COEFF_C_HIGH   (DLL_OMEGA_HIGH * DLL_OMEGA_HIGH)
41
42 // the low-bandwidth coefficients are used once we have a
43 // good estimate of the internal parameters
44 #define DLL_BANDWIDTH (0.1)
45 #define DLL_OMEGA     (2.0*DLL_PI*DLL_BANDWIDTH)
46 #define DLL_COEFF_B   (DLL_SQRT2 * DLL_OMEGA)
47 #define DLL_COEFF_C   (DLL_OMEGA * DLL_OMEGA)
48
49 // is 1 sec
50 #define UPDATES_WITH_HIGH_BANDWIDTH \
51          (1000000 / IEEE1394SERVICE_CYCLETIMER_DLL_UPDATE_INTERVAL_USEC)
52
53 IMPL_DEBUG_MODULE( CycleTimerHelper, CycleTimerHelper, DEBUG_LEVEL_NORMAL );
54
55 CycleTimerHelper::CycleTimerHelper(Ieee1394Service &parent, unsigned int update_period_us)
56     : m_Parent ( parent )
57     , m_ticks_per_update ( ((uint64_t)TICKS_PER_SECOND) * ((uint64_t)update_period_us) / 1000000ULL )
58     , m_usecs_per_update ( update_period_us )
59     , m_avg_wakeup_delay ( 0.0 )
60     , m_dll_e2 ( 0.0 )
61     , m_current_time_usecs ( 0 )
62     , m_next_time_usecs ( 0 )
63     , m_current_time_ticks ( 0 )
64     , m_next_time_ticks ( 0 )
65     , m_first_run ( true )
66     , m_sleep_until ( 0 )
67     , m_cycle_timer_prev ( 0 )
68     , m_cycle_timer_ticks_prev ( 0 )
69     , m_high_bw_updates ( UPDATES_WITH_HIGH_BANDWIDTH )
70     , m_current_shadow_idx ( 0 )
71     , m_Thread ( NULL )
72     , m_realtime ( false )
73     , m_priority ( 0 )
74     , m_busreset_functor ( NULL)
75     , m_unhandled_busreset ( false )
76 {
77     debugOutput( DEBUG_LEVEL_VERBOSE, "Create %p...\n", this);
78     pthread_mutex_init(&mb_update_lock, NULL);
79 }
80
81 CycleTimerHelper::CycleTimerHelper(Ieee1394Service &parent, unsigned int update_period_us, bool rt, int prio)
82     : m_Parent ( parent )
83     , m_ticks_per_update ( ((uint64_t)TICKS_PER_SECOND) * ((uint64_t)update_period_us) / 1000000ULL )
84     , m_usecs_per_update ( update_period_us )
85     , m_avg_wakeup_delay ( 0.0 )
86     , m_dll_e2 ( 0.0 )
87     , m_current_time_usecs ( 0 )
88     , m_next_time_usecs ( 0 )
89     , m_current_time_ticks ( 0 )
90     , m_next_time_ticks ( 0 )
91     , m_first_run ( true )
92     , m_sleep_until ( 0 )
93     , m_cycle_timer_prev ( 0 )
94     , m_cycle_timer_ticks_prev ( 0 )
95     , m_high_bw_updates ( UPDATES_WITH_HIGH_BANDWIDTH )
96     , m_current_shadow_idx ( 0 )
97     , m_Thread ( NULL )
98     , m_realtime ( rt )
99     , m_priority ( prio )
100     , m_busreset_functor ( NULL)
101     , m_unhandled_busreset ( false )
102 {
103     debugOutput( DEBUG_LEVEL_VERBOSE, "Create %p...\n", this);
104     pthread_mutex_init(&mb_update_lock, NULL);
105 }
106
107 CycleTimerHelper::~CycleTimerHelper()
108 {
109     if (m_Thread) {
110         m_Thread->Stop();
111         delete m_Thread;
112     }
113
114     // unregister the bus reset handler
115     if(m_busreset_functor) {
116         m_Parent.remBusResetHandler( m_busreset_functor );
117         delete m_busreset_functor;
118     }
119     pthread_mutex_destroy(&mb_update_lock);
120 }
121
122 bool
123 CycleTimerHelper::Start()
124 {
125     debugOutput( DEBUG_LEVEL_VERBOSE, "Start %p...\n", this);
126
127     if(!initValues()) {
128         debugFatal("(%p) Could not init values\n", this);
129         return false;
130     }
131
132     m_Thread = new Util::PosixThread(this, m_realtime, m_priority,
133                                      PTHREAD_CANCEL_DEFERRED);
134     if(!m_Thread) {
135         debugFatal("No thread\n");
136         return false;
137     }
138     // register the thread with the RT watchdog
139     Util::Watchdog *watchdog = m_Parent.getWatchdog();
140     if(watchdog) {
141         if(!watchdog->registerThread(m_Thread)) {
142             debugWarning("could not register update thread with watchdog\n");
143         }
144     } else {
145         debugWarning("could not find valid watchdog\n");
146     }
147    
148     if (m_Thread->Start() != 0) {
149         debugFatal("Could not start update thread\n");
150         return false;
151     }
152     return true;
153 }
154
155 bool
156 CycleTimerHelper::initValues()
157 {
158     debugOutput( DEBUG_LEVEL_VERBOSE, "(%p) Init values...\n", this );
159     pthread_mutex_lock(&mb_update_lock);
160     // initialize the 'prev ctr' values
161     uint64_t local_time;
162     int maxtries2 = 10;
163     do {
164         debugOutput( DEBUG_LEVEL_VERBOSE, "Read CTR...\n" );
165         if(!m_Parent.readCycleTimerReg(&m_cycle_timer_prev, &local_time)) {
166             debugError("Could not read cycle timer register\n");
167             pthread_mutex_unlock(&mb_update_lock);
168             return false;
169         }
170         if (m_cycle_timer_prev == 0) {
171             debugOutput(DEBUG_LEVEL_VERBOSE,
172                         "Bogus CTR: %08X on try %02d\n",
173                         m_cycle_timer_prev, maxtries2);
174         }
175         debugOutput( DEBUG_LEVEL_VERBOSE, " read : CTR: %11lu, local: %17llu\n",
176                             m_cycle_timer_prev, local_time);
177         debugOutput(DEBUG_LEVEL_VERBOSE,
178                            "  ctr   : 0x%08X %11llu (%03us %04ucy %04uticks)\n",
179                            (uint32_t)m_cycle_timer_prev, (uint64_t)CYCLE_TIMER_TO_TICKS(m_cycle_timer_prev),
180                            (unsigned int)CYCLE_TIMER_GET_SECS( m_cycle_timer_prev ),
181                            (unsigned int)CYCLE_TIMER_GET_CYCLES( m_cycle_timer_prev ),
182                            (unsigned int)CYCLE_TIMER_GET_OFFSET( m_cycle_timer_prev ) );
183        
184     } while (m_cycle_timer_prev == 0 && maxtries2--);
185     m_cycle_timer_ticks_prev = CYCLE_TIMER_TO_TICKS(m_cycle_timer_prev);
186
187 #if IEEE1394SERVICE_USE_CYCLETIMER_DLL
188     debugOutput( DEBUG_LEVEL_VERBOSE, "requesting DLL re-init...\n" );
189     m_TimeSource.SleepUsecRelative(1000); // some time to settle
190     m_high_bw_updates = UPDATES_WITH_HIGH_BANDWIDTH;
191     if(!initDLL()) {
192         debugError("(%p) Could not init DLL\n", this);
193         pthread_mutex_unlock(&mb_update_lock);
194         return false;
195     }
196     // make the DLL re-init itself as if it were started up
197     m_first_run = true;
198 #endif
199     pthread_mutex_unlock(&mb_update_lock);
200     debugOutput( DEBUG_LEVEL_VERBOSE, "ready...\n" );
201     return true;
202 }
203
204 bool
205 CycleTimerHelper::Init()
206 {
207     debugOutput( DEBUG_LEVEL_VERBOSE, "Initialize %p...\n", this);
208
209     // register a bus reset handler
210     Util::Functor* m_busreset_functor = new Util::MemberFunctor0< CycleTimerHelper*,
211                 void (CycleTimerHelper::*)() >
212                 ( this, &CycleTimerHelper::busresetHandler, false );
213     if ( !m_busreset_functor ) {
214         debugFatal( "(%p) Could not create busreset handler\n", this );
215         return false;
216     }
217     m_Parent.addBusResetHandler( m_busreset_functor );
218
219     #ifdef DEBUG
220     m_last_loop_entry = 0;
221     m_successive_short_loops = 0;
222     #endif
223
224     return true;
225 }
226
227 void
228 CycleTimerHelper::busresetHandler()
229 {
230     debugOutput( DEBUG_LEVEL_VERBOSE, "Bus reset...\n" );
231     m_unhandled_busreset = true;
232     // whenever a bus reset occurs, the root node can change,
233     // and the CTR timer can be reset. We should hence reinit
234     // the DLL
235     if(!initValues()) {
236         debugError("(%p) Could not re-init values\n", this);
237     }
238     m_unhandled_busreset = false;
239 }
240
241 bool
242 CycleTimerHelper::setThreadParameters(bool rt, int priority) {
243     debugOutput( DEBUG_LEVEL_VERBOSE, "(%p) switch to: (rt=%d, prio=%d)...\n", this, rt, priority);
244     if (priority > THREAD_MAX_RTPRIO) priority = THREAD_MAX_RTPRIO; // cap the priority
245     m_realtime = rt;
246     m_priority = priority;
247
248 #if IEEE1394SERVICE_USE_CYCLETIMER_DLL
249     if (m_Thread) {
250         if (m_realtime) {
251             m_Thread->AcquireRealTime(m_priority);
252         } else {
253             m_Thread->DropRealTime();
254         }
255     }
256 #endif
257
258     return true;
259 }
260
261 #if IEEE1394SERVICE_USE_CYCLETIMER_DLL
262 float
263 CycleTimerHelper::getRate()
264 {
265     float rate = (float)(diffTicks((uint64_t)m_next_time_ticks, (uint64_t)m_current_time_ticks));
266     rate /= (float)(m_next_time_usecs - m_current_time_usecs);
267     return rate;
268 }
269
270 float
271 CycleTimerHelper::getNominalRate()
272 {
273     float rate = ((double)TICKS_PER_SECOND) / 1000000.0;
274     return rate;
275 }
276
277 bool
278 CycleTimerHelper::initDLL() {
279     uint32_t cycle_timer;
280     uint64_t local_time;
281     uint64_t cycle_timer_ticks;
282
283     if(!readCycleTimerWithRetry(&cycle_timer, &local_time, 10)) {
284         debugError("Could not read cycle timer register\n");
285         pthread_mutex_unlock(&mb_update_lock);
286         return false;
287     }
288     cycle_timer_ticks = CYCLE_TIMER_TO_TICKS(cycle_timer);
289
290     debugOutputExtreme( DEBUG_LEVEL_VERY_VERBOSE, " read : CTR: %11lu, local: %17llu\n",
291                         cycle_timer, local_time);
292     debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE,
293                        "  ctr   : 0x%08X %11llu (%03us %04ucy %04uticks)\n",
294                        (uint32_t)cycle_timer, (uint64_t)cycle_timer_ticks,
295                        (unsigned int)TICKS_TO_SECS( (uint64_t)cycle_timer_ticks ),
296                        (unsigned int)TICKS_TO_CYCLES( (uint64_t)cycle_timer_ticks ),
297                        (unsigned int)TICKS_TO_OFFSET( (uint64_t)cycle_timer_ticks ) );
298
299     m_sleep_until = local_time + m_usecs_per_update;
300     m_dll_e2 = m_ticks_per_update;
301     m_current_time_usecs = local_time;
302     m_next_time_usecs = m_current_time_usecs + m_usecs_per_update;
303     m_current_time_ticks = CYCLE_TIMER_TO_TICKS( cycle_timer );
304     m_next_time_ticks = addTicks( (uint64_t)m_current_time_ticks, (uint64_t)m_dll_e2);
305     debugOutput(DEBUG_LEVEL_VERBOSE, " (%p) First run\n", this);
306     debugOutput(DEBUG_LEVEL_VERBOSE,
307                 "  usecs/update: %lu, ticks/update: %lu, m_dll_e2: %f\n",
308                 m_usecs_per_update, m_ticks_per_update, m_dll_e2);
309     debugOutput(DEBUG_LEVEL_VERBOSE,
310                 "  usecs current: %f, next: %f\n",
311                 m_current_time_usecs, m_next_time_usecs);
312     debugOutput(DEBUG_LEVEL_VERBOSE,
313                 "  ticks current: %f, next: %f\n",
314                 m_current_time_ticks, m_next_time_ticks);
315     return true;
316 }
317
318 bool
319 CycleTimerHelper::Execute()
320 {
321     debugOutputExtreme( DEBUG_LEVEL_VERY_VERBOSE, "Execute %p...\n", this);
322
323     #ifdef DEBUG
324     uint64_t now = m_Parent.getCurrentTimeAsUsecs();
325     int diff = now - m_last_loop_entry;
326     if(diff < 100) {
327         debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE,
328                            "(%p) short loop detected (%d usec), cnt: %d\n",
329                            this, diff, m_successive_short_loops);
330         m_successive_short_loops++;
331         if(m_successive_short_loops > 100) {
332             debugError("Shutting down runaway thread\n");
333             return false;
334         }
335     } else {
336         // reset the counter
337         m_successive_short_loops = 0;
338     }
339     m_last_loop_entry = now;
340     #endif
341
342     if (!m_first_run) {
343         // wait for the next update period
344         #if DEBUG_EXTREME_ENABLE
345         ffado_microsecs_t now = m_TimeSource.getCurrentTimeAsUsecs();
346         int sleep_time = m_sleep_until - now;
347         debugOutputExtreme( DEBUG_LEVEL_VERY_VERBOSE, "(%p) Sleep until %lld/%f (now: %lld, diff=%d) ...\n",
348                     this, m_sleep_until, m_next_time_usecs, now, sleep_time);
349         #endif
350         m_TimeSource.SleepUsecAbsolute(m_sleep_until);
351         debugOutputExtreme( DEBUG_LEVEL_VERY_VERBOSE, " (%p) back...\n", this);
352     }
353
354     uint32_t cycle_timer;
355     uint64_t local_time;
356     int64_t usecs_late;
357     int ntries=4;
358     uint64_t cycle_timer_ticks;
359     double diff_ticks;
360
361     // if the difference between the predicted value and the
362     // actual value seems to be too large, retry reading the cycle timer
363     // some host controllers return bogus values on some reads
364     // (looks like a non-atomic update of the register)
365     do {
366         if(!readCycleTimerWithRetry(&cycle_timer, &local_time, 10)) {
367             debugError("Could not read cycle timer register\n");
368             return false;
369         }
370         usecs_late = local_time - m_sleep_until;
371         cycle_timer_ticks = CYCLE_TIMER_TO_TICKS(cycle_timer);
372         diff_ticks = diffTicks(cycle_timer_ticks, (int64_t)m_next_time_ticks);
373
374         // check for unrealistic CTR reads (NEC controller does that sometimes)
375         if(diff_ticks < -((double)TICKS_PER_HALFCYCLE)) {
376             debugOutput(DEBUG_LEVEL_VERBOSE,
377                         "(%p) have to retry CTR read, diff unrealistic: diff: %f, max: %f (try: %d)\n",
378                         this, diff_ticks, -((double)TICKS_PER_HALFCYCLE), ntries);
379         }
380
381     } while( diff_ticks < -((double)TICKS_PER_HALFCYCLE)
382              && --ntries && !m_first_run && !m_unhandled_busreset);
383
384     // grab the lock after sleeping, otherwise we can't be interrupted by
385     // the busreset thread (lower prio)
386     // also grab it after reading the CTR register such that the jitter between
387     // wakeup and read is as small as possible
388     pthread_mutex_lock(&mb_update_lock);
389
390     // // simulate a random scheduling delay between (0-10ms)
391     // ffado_microsecs_t tmp = m_TimeSource.SleepUsecRandom(10000);
392     // debugOutput( DEBUG_LEVEL_VERBOSE, " (%p) random sleep of %llu usecs...\n", this, tmp);
393
394     if(m_unhandled_busreset) {
395         debugOutput(DEBUG_LEVEL_VERBOSE,
396                     "(%p) Skipping DLL update due to unhandled busreset\n", this);
397         m_sleep_until += m_usecs_per_update;
398         pthread_mutex_unlock(&mb_update_lock);
399         // keep the thread running
400         return true;
401     }
402
403     debugOutputExtreme( DEBUG_LEVEL_VERY_VERBOSE, " read : CTR: %11lu, local: %17llu\n",
404                         cycle_timer, local_time);
405     debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE,
406                        "  ctr   : 0x%08X %11llu (%03us %04ucy %04uticks)\n",
407                        (uint32_t)cycle_timer, (uint64_t)cycle_timer_ticks,
408                        (unsigned int)TICKS_TO_SECS( (uint64_t)cycle_timer_ticks ),
409                        (unsigned int)TICKS_TO_CYCLES( (uint64_t)cycle_timer_ticks ),
410                        (unsigned int)TICKS_TO_OFFSET( (uint64_t)cycle_timer_ticks ) );
411
412     if (m_first_run) {
413         if(!initDLL()) {
414             debugError("(%p) Could not init DLL\n", this);
415             pthread_mutex_unlock(&mb_update_lock);
416             return false;
417         }
418         m_first_run = false;
419     } else {
420         // calculate next sleep time
421         m_sleep_until += m_usecs_per_update;
422
423         // correct for the latency between the wakeup and the actual CTR
424         // read. The only time we can trust is the time returned by the
425         // CTR read kernel call, since that (should be) atomically read
426         // together with the ctr register itself.
427
428         // if we are usecs_late usecs late
429         // the cycle timer has ticked approx ticks_late ticks too much
430         // if we are woken up early (which shouldn't happen according to POSIX)
431         // the cycle timer has ticked approx -ticks_late too little
432         int64_t ticks_late = (usecs_late * TICKS_PER_SECOND) / 1000000LL;
433         // the corrected difference between predicted and actual ctr
434         // i.e. DLL error signal
435         double diff_ticks_corr;
436         if (ticks_late > 0) {
437             diff_ticks_corr = diff_ticks - ticks_late;
438             debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE,
439                                "diff_ticks_corr=%f, diff_ticks = %f, ticks_late = %lld\n",
440                                diff_ticks_corr, diff_ticks, ticks_late);
441         } else {
442             debugError("Early wakeup, should not happen!\n");
443             // recover
444             diff_ticks_corr = diff_ticks + ticks_late;
445         }
446
447         #ifdef DEBUG
448         // makes no sense if not running realtime
449         if(m_realtime && usecs_late > 200) {
450             debugOutput(DEBUG_LEVEL_VERBOSE, "Rather late wakeup: %lld usecs\n", usecs_late);
451         }
452         #endif
453
454         // update the x-axis values
455         m_current_time_ticks = m_next_time_ticks;
456
457         // decide what coefficients to use
458         double coeff_b, coeff_c;
459         if (m_high_bw_updates > 0) {
460             coeff_b = DLL_COEFF_B_HIGH;
461             coeff_c = DLL_COEFF_C_HIGH;
462             m_high_bw_updates--;
463             if (m_high_bw_updates == 0) {
464                 debugOutput(DEBUG_LEVEL_VERBOSE,
465                             "Switching to low-bandwidth coefficients\n");
466             }
467         } else {
468             coeff_b = DLL_COEFF_B;
469             coeff_c = DLL_COEFF_C;
470         }
471
472         // it should be ok to not do this in tick space
473         // since diff_ticks_corr should not be near wrapping
474         // (otherwise we are out of range. we need a few calls
475         //  w/o wrapping for this to work. That should not be
476         //  an issue as long as the update interval is smaller
477         //  than the wrapping interval.)
478         // and coeff_b < 1, hence tmp is not near wrapping
479
480         double step_ticks = (coeff_b * diff_ticks_corr);
481         debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE,
482                            "diff_ticks_corr=%f, step_ticks=%f\n",
483                            diff_ticks_corr, step_ticks);
484
485         // the same goes for m_dll_e2, which should be approx equal
486         // to the ticks/usec rate (= 24.576) hence also not near
487         // wrapping
488         step_ticks += m_dll_e2;
489         debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE,
490                            "add %f ticks to step_ticks => step_ticks=%f\n",
491                            m_dll_e2, step_ticks);
492
493         // it can't be that we have to update to a value in the past
494         if(step_ticks < 0) {
495             debugError("negative step: %f! (correcting to nominal)\n", step_ticks);
496             // recover to an estimated value
497             step_ticks = (double)m_ticks_per_update;
498         }
499
500         if(step_ticks > TICKS_PER_SECOND) {
501             debugWarning("rather large step: %f ticks (> 1sec)\n", step_ticks);
502         }
503
504         // now add the step ticks with wrapping.
505         m_next_time_ticks = (double)(addTicks((uint64_t)m_current_time_ticks, (uint64_t)step_ticks));
506
507         // update the DLL state
508         m_dll_e2 += coeff_c * diff_ticks_corr;
509
510         // update the y-axis values
511         m_current_time_usecs = m_next_time_usecs;
512         m_next_time_usecs += m_usecs_per_update;
513
514         debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE,
515                            " usecs: current: %f next: %f usecs_late=%lld ticks_late=%lld\n",
516                            m_current_time_usecs, m_next_time_usecs, usecs_late, ticks_late);
517         debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE,
518                            " ticks: current: %f next: %f diff=%f\n",
519                            m_current_time_ticks, m_next_time_ticks, diff_ticks);
520         debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE,
521                            " ticks: current: %011llu (%03us %04ucy %04uticks)\n",
522                            (uint64_t)m_current_time_ticks,
523                            (unsigned int)TICKS_TO_SECS( (uint64_t)m_current_time_ticks ),
524                            (unsigned int)TICKS_TO_CYCLES( (uint64_t)m_current_time_ticks ),
525                            (unsigned int)TICKS_TO_OFFSET( (uint64_t)m_current_time_ticks ) );
526         debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE,
527                            " ticks: next   : %011llu (%03us %04ucy %04uticks)\n",
528                            (uint64_t)m_next_time_ticks,
529                            (unsigned int)TICKS_TO_SECS( (uint64_t)m_next_time_ticks ),
530                            (unsigned int)TICKS_TO_CYCLES( (uint64_t)m_next_time_ticks ),
531                            (unsigned int)TICKS_TO_OFFSET( (uint64_t)m_next_time_ticks ) );
532
533         debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE,
534                            " state: local: %11llu, dll_e2: %f, rate: %f\n",
535                            local_time, m_dll_e2, getRate());
536     }
537
538     // prepare the new compute vars
539     struct compute_vars new_vars;
540     new_vars.ticks = (uint64_t)(m_current_time_ticks);
541     new_vars.usecs = (uint64_t)m_current_time_usecs;
542     new_vars.rate = getRate();
543
544     // get the next index
545     unsigned int next_idx = (m_current_shadow_idx + 1) % CTRHELPER_NB_SHADOW_VARS;
546
547     // update the next index position
548     m_shadow_vars[next_idx] = new_vars;
549
550     // then we can update the current index
551     m_current_shadow_idx = next_idx;
552
553     pthread_mutex_unlock(&mb_update_lock);
554
555 #ifdef DEBUG
556     // do some verification
557     // we re-read a valid ctr timestamp
558     // then we use the attached system time to calculate
559     // the DLL generated timestamp and we check what the
560     // difference is
561
562     if(!readCycleTimerWithRetry(&cycle_timer, &local_time, 10)) {
563         debugError("Could not read cycle timer register (verify)\n");
564         return true; // true since this is a check only
565     }
566     cycle_timer_ticks = CYCLE_TIMER_TO_TICKS(cycle_timer);
567
568     // only check when successful
569     int64_t time_diff = local_time - new_vars.usecs;
570     double y_step_in_ticks = ((double)time_diff) * new_vars.rate;
571     int64_t y_step_in_ticks_int = (int64_t)y_step_in_ticks;
572     uint64_t offset_in_ticks_int = new_vars.ticks;
573     uint32_t dll_time;
574     if (y_step_in_ticks_int > 0) {
575         dll_time = addTicks(offset_in_ticks_int, y_step_in_ticks_int);
576     } else {
577         dll_time = substractTicks(offset_in_ticks_int, -y_step_in_ticks_int);
578     }
579     int32_t ctr_diff = cycle_timer_ticks-dll_time;
580     debugOutput(DEBUG_LEVEL_VERY_VERBOSE, "(%p) CTR DIFF: HW %010llu - DLL %010lu = %010ld (%s)\n",
581                 this, cycle_timer_ticks, dll_time, ctr_diff, (ctr_diff>0?"lag":"lead"));
582 #endif
583
584     return true;
585 }
586
587 uint32_t
588 CycleTimerHelper::getCycleTimerTicks()
589 {
590     uint64_t now = m_Parent.getCurrentTimeAsUsecs();
591     return getCycleTimerTicks(now);
592 }
593
594 uint32_t
595 CycleTimerHelper::getCycleTimerTicks(uint64_t now)
596 {
597     uint32_t retval;
598     struct compute_vars *my_vars;
599
600     // get pointer and copy the contents
601     // no locking should be needed since we have more than one
602     // of these vars available, and our use will always be finished before
603     // m_current_shadow_idx changes since this thread's priority should
604     // be higher than the one of the writer thread. Even if not, we only have to ensure
605     // that the used dataset is consistent. We can use an older dataset if it's consistent
606     // since it will also provide a fairly decent extrapolation.
607     my_vars = m_shadow_vars + m_current_shadow_idx;
608
609     int64_t time_diff = now - my_vars->usecs;
610     double y_step_in_ticks = ((double)time_diff) * my_vars->rate;
611     int64_t y_step_in_ticks_int = (int64_t)y_step_in_ticks;
612     uint64_t offset_in_ticks_int = my_vars->ticks;
613
614     if (y_step_in_ticks_int > 0) {
615         retval = addTicks(offset_in_ticks_int, y_step_in_ticks_int);
616 /*        debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE, "y_step_in_ticks_int > 0: %lld, time_diff: %f, rate: %f, retval: %lu\n",
617                     y_step_in_ticks_int, time_diff, my_vars.rate, retval);*/
618     } else {
619         retval = substractTicks(offset_in_ticks_int, -y_step_in_ticks_int);
620
621         // this can happen if the update thread was woken up earlier than it should have been
622 /*        debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE, "y_step_in_ticks_int <= 0: %lld, time_diff: %f, rate: %f, retval: %lu\n",
623                     y_step_in_ticks_int, time_diff, my_vars.rate, retval);*/
624     }
625
626     return retval;
627 }
628
629 uint32_t
630 CycleTimerHelper::getCycleTimer()
631 {
632     uint64_t now = m_Parent.getCurrentTimeAsUsecs();
633     return getCycleTimer(now);
634 }
635
636 uint32_t
637 CycleTimerHelper::getCycleTimer(uint64_t now)
638 {
639     uint32_t ticks = getCycleTimerTicks(now);
640     uint32_t result = TICKS_TO_CYCLE_TIMER(ticks);
641 #ifdef DEBUG
642     if(CYCLE_TIMER_TO_TICKS(result) != ticks) {
643         debugWarning("Bad ctr conversion");
644     }
645 #endif
646     return result;
647 }
648
649 #else
650
651 float
652 CycleTimerHelper::getRate()
653 {
654     return getNominalRate();
655 }
656
657 float
658 CycleTimerHelper::getNominalRate()
659 {
660     float rate = ((double)TICKS_PER_SECOND) / 1000000.0;
661     return rate;
662 }
663
664 bool
665 CycleTimerHelper::Execute()
666 {
667     usleep(1000*1000);
668     return true;
669 }
670
671 uint32_t
672 CycleTimerHelper::getCycleTimerTicks()
673 {
674     return CYCLE_TIMER_TO_TICKS(getCycleTimer());
675 }
676
677 uint32_t
678 CycleTimerHelper::getCycleTimerTicks(uint64_t now)
679 {
680     debugWarning("not implemented!\n");
681     return getCycleTimerTicks();
682 }
683
684 uint32_t
685 CycleTimerHelper::getCycleTimer()
686 {
687     uint32_t cycle_timer;
688     uint64_t local_time;
689     readCycleTimerWithRetry(&cycle_timer, &local_time, 10);
690     return cycle_timer;
691 }
692
693 uint32_t
694 CycleTimerHelper::getCycleTimer(uint64_t now)
695 {
696     debugWarning("not implemented!\n");
697     return getCycleTimer();
698 }
699
700 #endif
701
702 bool
703 CycleTimerHelper::readCycleTimerWithRetry(uint32_t *cycle_timer, uint64_t *local_time, int ntries)
704 {
705     bool good=false;
706     int maxtries = ntries;
707
708     do {
709         // the ctr read can return 0 sometimes. if that happens, reread the ctr.
710         int maxtries2=ntries;
711         do {
712             if(!m_Parent.readCycleTimerReg(cycle_timer, local_time)) {
713                 debugError("Could not read cycle timer register\n");
714                 return false;
715             }
716             if (*cycle_timer == 0) {
717                 debugOutput(DEBUG_LEVEL_VERBOSE,
718                            "Bogus CTR: %08X on try %02d\n",
719                            *cycle_timer, maxtries2);
720             }
721         } while (*cycle_timer == 0 && maxtries2--);
722        
723         // catch bogus ctr reads (can happen)
724         uint64_t cycle_timer_ticks = CYCLE_TIMER_TO_TICKS(*cycle_timer);
725    
726         if (diffTicks(cycle_timer_ticks, m_cycle_timer_ticks_prev) < 0) {
727             debugOutput( DEBUG_LEVEL_VERY_VERBOSE,
728                         "non-monotonic CTR (try %02d): %llu -> %llu\n",
729                         maxtries, m_cycle_timer_ticks_prev, cycle_timer_ticks);
730             debugOutput( DEBUG_LEVEL_VERY_VERBOSE,
731                         "                            : %08X -> %08X\n",
732                         m_cycle_timer_prev, *cycle_timer);
733             debugOutput( DEBUG_LEVEL_VERY_VERBOSE,
734                         " current: %011llu (%03us %04ucy %04uticks)\n",
735                         cycle_timer_ticks,
736                         (unsigned int)TICKS_TO_SECS( cycle_timer_ticks ),
737                         (unsigned int)TICKS_TO_CYCLES( cycle_timer_ticks ),
738                         (unsigned int)TICKS_TO_OFFSET( cycle_timer_ticks ) );
739             debugOutput( DEBUG_LEVEL_VERY_VERBOSE,
740                         " prev   : %011llu (%03us %04ucy %04uticks)\n",
741                         m_cycle_timer_ticks_prev,
742                         (unsigned int)TICKS_TO_SECS( m_cycle_timer_ticks_prev ),
743                         (unsigned int)TICKS_TO_CYCLES( m_cycle_timer_ticks_prev ),
744                         (unsigned int)TICKS_TO_OFFSET( m_cycle_timer_ticks_prev ) );
745         } else {
746             good = true;
747             m_cycle_timer_prev = *cycle_timer;
748             m_cycle_timer_ticks_prev = cycle_timer_ticks;
749         }
750     } while (!good && maxtries--);
751     return true;
752 }
753
754 void
755 CycleTimerHelper::setVerboseLevel(int l)
756 {
757     setDebugLevel(l);
758 }
Note: See TracBrowser for help on using the browser.