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

Revision 2179, 30.6 kB (checked in by jwoithe, 12 years ago)

Fix 'insufficient wrapping' warnings which could occur during startup due to uninitialised shadow variables. Whether this had anything to do with intermittant startup failures observed from time to time remains to be seen.

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