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

Revision 2651, 30.7 kB (checked in by jwoithe, 4 years ago)

Fix some warnings emitted by recent versions of gcc. These are mostly connected with printf format strings: spaces are now required between string literals and PR* format macros. Patch from Xavier Forestier.

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
273     double bw_rel = m_dll_coeff_b / (DLL_SQRT2 * DLL_2PI);
274     double bw_abs = bw_rel / (m_usecs_per_update / 1e6);
275     if (bw_rel > 0.5) {
276         double bw_max = 0.5 / (m_usecs_per_update / 1e6);
277         debugWarning("Specified DLL bandwidth too high (%f > %f), reducing to max."
278                      " Increase the DLL update rate to increase the max DLL bandwidth\n", bw_abs, bw_max);
279
280         bw_rel = 0.49;
281         bw_abs = bw_rel / (m_usecs_per_update / 1e6);
282         m_dll_coeff_b = bw_rel * (DLL_SQRT2 * DLL_2PI);
283         m_dll_coeff_c = bw_rel * bw_rel * DLL_2PI * DLL_2PI;
284     }
285
286     if(!readCycleTimerWithRetry(&cycle_timer, &local_time, 10)) {
287         debugError("Could not read cycle timer register\n");
288         return false;
289     }
290     #if DEBUG_EXTREME_ENABLE
291     uint64_t cycle_timer_ticks = CYCLE_TIMER_TO_TICKS(cycle_timer);
292     #endif
293
294     debugOutputExtreme( DEBUG_LEVEL_VERY_VERBOSE, " read : CTR: %11u, local: %17" PRIu64 "\n",
295                         cycle_timer, local_time);
296     debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE,
297                        "  ctr   : 0x%08X %11" PRIu64 " (%03us %04ucy %04uticks)\n",
298                        (uint32_t)cycle_timer, (uint64_t)cycle_timer_ticks,
299                        (unsigned int)TICKS_TO_SECS( (uint64_t)cycle_timer_ticks ),
300                        (unsigned int)TICKS_TO_CYCLES( (uint64_t)cycle_timer_ticks ),
301                        (unsigned int)TICKS_TO_OFFSET( (uint64_t)cycle_timer_ticks ) );
302
303     m_sleep_until = local_time + m_usecs_per_update;
304     m_dll_e2 = m_ticks_per_update;
305     m_current_time_usecs = local_time;
306     m_next_time_usecs = m_current_time_usecs + m_usecs_per_update;
307     m_current_time_ticks = CYCLE_TIMER_TO_TICKS( cycle_timer );
308     m_next_time_ticks = addTicks( (uint64_t)m_current_time_ticks, (uint64_t)m_dll_e2);
309     debugOutput(DEBUG_LEVEL_VERBOSE, " (%p) First run\n", this);
310     debugOutput(DEBUG_LEVEL_VERBOSE, "  DLL bandwidth: %f Hz (rel: %f)\n",
311                 bw_abs, bw_rel);
312     debugOutput(DEBUG_LEVEL_VERBOSE,
313                 "  usecs/update: %u, ticks/update: %u, m_dll_e2: %f\n",
314                 m_usecs_per_update, m_ticks_per_update, m_dll_e2);
315     debugOutput(DEBUG_LEVEL_VERBOSE,
316                 "  usecs current: %f, next: %f\n",
317                 m_current_time_usecs, m_next_time_usecs);
318     debugOutput(DEBUG_LEVEL_VERBOSE,
319                 "  ticks current: %f, next: %f\n",
320                 m_current_time_ticks, m_next_time_ticks);
321     return true;
322 }
323
324 bool
325 CycleTimerHelper::Execute()
326 {
327     debugOutput( DEBUG_LEVEL_ULTRA_VERBOSE, "Execute %p...\n", this);
328
329     #ifdef DEBUG
330     uint64_t now = m_Parent.getCurrentTimeAsUsecs();
331     int diff = now - m_last_loop_entry;
332     if(diff < 100) {
333         debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE,
334                         "(%p) short loop detected (%d usec), cnt: %d\n",
335                         this, diff, m_successive_short_loops);
336         m_successive_short_loops++;
337         if(m_successive_short_loops > 100) {
338             debugError("Shutting down runaway thread\n");
339             return false;
340         }
341     } else {
342         // reset the counter
343         m_successive_short_loops = 0;
344     }
345     m_last_loop_entry = now;
346     #endif
347
348     if (!m_first_run) {
349         // wait for the next update period
350         //#if DEBUG_EXTREME_ENABLE
351         #ifdef DEBUG
352         ffado_microsecs_t now = Util::SystemTimeSource::getCurrentTimeAsUsecs();
353         int sleep_time = m_sleep_until - now;
354         debugOutput( DEBUG_LEVEL_ULTRA_VERBOSE, "(%p) Sleep until %" PRId64 "/%f (now: %" PRId64 ", diff=%d) ...\n",
355                     this, m_sleep_until, m_next_time_usecs, now, sleep_time);
356         #endif
357         Util::SystemTimeSource::SleepUsecAbsolute(m_sleep_until);
358         debugOutput( DEBUG_LEVEL_ULTRA_VERBOSE, " (%p) back...\n", this);
359     } else {
360         // Since getCycleTimerTicks() is called below,
361         // m_shadow_vars[m_current_shadow_idx] must contain valid data.  On
362         // the first run through, however, it won't because the contents of
363         // m_shadow_vars[] are only set later on in this function.  Thus
364         // set up some vaguely realistic values to prevent unnecessary
365         // delays when reading the cycle timer for the first time.
366         struct compute_vars new_vars;
367         new_vars.ticks = (uint64_t)(m_current_time_ticks);
368         new_vars.usecs = (uint64_t)m_current_time_usecs;
369         new_vars.rate = getRate();
370         m_shadow_vars[0] = new_vars;
371     }
372
373     uint32_t cycle_timer;
374     uint64_t local_time;
375     int64_t usecs_late;
376     int ntries=10;
377     uint64_t cycle_timer_ticks;
378     int64_t err_ticks;
379     bool not_good;
380
381     // if the difference between the predicted value at readout time and the
382     // actual value seems to be too large, retry reading the cycle timer
383     // some host controllers return bogus values on some reads
384     // (looks like a non-atomic update of the register)
385     do {
386         debugOutput( DEBUG_LEVEL_ULTRA_VERBOSE, "(%p) reading cycle timer register...\n", this);
387         if(!readCycleTimerWithRetry(&cycle_timer, &local_time, 10)) {
388             debugError("Could not read cycle timer register\n");
389             return false;
390         }
391         usecs_late = local_time - m_sleep_until;
392         cycle_timer_ticks = CYCLE_TIMER_TO_TICKS(cycle_timer);
393
394         // calculate the CTR_TICKS we expect to read at "local_time"
395         // then calculate the difference with what we actually read,
396         // taking wraparound into account. If these deviate too much
397         // from eachother then read the register again (bogus read).
398         int64_t expected_ticks = getCycleTimerTicks(local_time);
399         err_ticks = diffTicks(cycle_timer_ticks, expected_ticks);
400
401         // check for unrealistic CTR reads (NEC controller does that sometimes)
402         not_good = (-err_ticks > 1*TICKS_PER_CYCLE || err_ticks > 1*TICKS_PER_CYCLE);
403         if(not_good) {
404             debugOutput(DEBUG_LEVEL_VERBOSE,
405                         "(%p) have to retry CTR read, diff unrealistic: diff: %" PRId64 ", max: +/- %u (try: %d) %" PRId64 "\n",
406                         this, err_ticks, 1*TICKS_PER_CYCLE, ntries, expected_ticks);
407             // sleep half a cycle to make sure the hardware moved on
408             Util::SystemTimeSource::SleepUsecRelative(USECS_PER_CYCLE / 2);
409         }
410
411     } while(not_good && --ntries && !m_first_run && !m_unhandled_busreset);
412
413     // grab the lock after sleeping, otherwise we can't be interrupted by
414     // the busreset thread (lower prio)
415     // also grab it after reading the CTR register such that the jitter between
416     // wakeup and read is as small as possible
417     Util::MutexLockHelper lock(*m_update_lock);
418
419     // the difference between the measured and the expected time
420     int64_t diff_ticks = diffTicks(cycle_timer_ticks, (int64_t)m_next_time_ticks);
421
422     // // simulate a random scheduling delay between (0-10ms)
423     // ffado_microsecs_t tmp = Util::SystemTimeSource::SleepUsecRandom(10000);
424     // debugOutput( DEBUG_LEVEL_VERBOSE, " (%p) random sleep of %u usecs...\n", this, tmp);
425
426     if(m_unhandled_busreset) {
427         debugOutput(DEBUG_LEVEL_VERBOSE,
428                     "(%p) Skipping DLL update due to unhandled busreset\n", this);
429         m_sleep_until += m_usecs_per_update;
430         // keep the thread running
431         return true;
432     }
433
434     debugOutputExtreme( DEBUG_LEVEL_ULTRA_VERBOSE, " read : CTR: %11u, local: %17" PRIu64 "\n",
435                         cycle_timer, local_time);
436     debugOutputExtreme(DEBUG_LEVEL_ULTRA_VERBOSE,
437                        "  ctr   : 0x%08X %11" PRIu64 " (%03us %04ucy %04uticks)\n",
438                        (uint32_t)cycle_timer, (uint64_t)cycle_timer_ticks,
439                        (unsigned int)TICKS_TO_SECS( (uint64_t)cycle_timer_ticks ),
440                        (unsigned int)TICKS_TO_CYCLES( (uint64_t)cycle_timer_ticks ),
441                        (unsigned int)TICKS_TO_OFFSET( (uint64_t)cycle_timer_ticks ) );
442
443     if (m_first_run) {
444         if(!initDLL()) {
445             debugError("(%p) Could not init DLL\n", this);
446             return false;
447         }
448         m_first_run = false;
449     } else if (diff_ticks > m_ticks_per_update * 20) {
450         debugOutput(DEBUG_LEVEL_VERBOSE,
451                     "re-init dll due to too large tick diff: %" PRId64 " >> %f\n",
452                     diff_ticks, (float)(m_ticks_per_update * 20));
453         if(!initDLL()) {
454             debugError("(%p) Could not init DLL\n", this);
455             return false;
456         }
457     } else {
458         // calculate next sleep time
459         m_sleep_until += m_usecs_per_update;
460
461         // correct for the latency between the wakeup and the actual CTR
462         // read. The only time we can trust is the time returned by the
463         // CTR read kernel call, since that (should be) atomically read
464         // together with the ctr register itself.
465
466         // if we are usecs_late usecs late
467         // the cycle timer has ticked approx ticks_late ticks too much
468         // if we are woken up early (which shouldn't happen according to POSIX)
469         // the cycle timer has ticked approx -ticks_late too little
470         int64_t ticks_late = (usecs_late * TICKS_PER_SECOND) / 1000000LL;
471         // the corrected difference between predicted and actual ctr
472         // i.e. DLL error signal
473         int64_t diff_ticks_corr;
474         if (ticks_late >= 0) {
475             diff_ticks_corr = diff_ticks - ticks_late;
476             debugOutputExtreme(DEBUG_LEVEL_ULTRA_VERBOSE,
477                                "diff_ticks_corr=%" PRId64 ", diff_ticks = %" PRId64 ", ticks_late = %" PRId64 "\n",
478                                diff_ticks_corr, diff_ticks, ticks_late);
479         } else {
480             debugError("Early wakeup, should not happen!\n");
481             // recover
482             diff_ticks_corr = diff_ticks + ticks_late;
483         }
484
485         #ifdef DEBUG
486         // makes no sense if not running realtime
487         if(m_realtime && usecs_late > 1000) {
488             debugOutput(DEBUG_LEVEL_VERBOSE, "Rather late wakeup: %" PRId64 " usecs\n", usecs_late);
489         }
490         #endif
491
492         // update the x-axis values
493         m_current_time_ticks = m_next_time_ticks;
494
495         // decide what coefficients to use
496
497         // it should be ok to not do this in tick space
498         // since diff_ticks_corr should not be near wrapping
499         // (otherwise we are out of range. we need a few calls
500         //  w/o wrapping for this to work. That should not be
501         //  an issue as long as the update interval is smaller
502         //  than the wrapping interval.)
503         // and coeff_b < 1, hence tmp is not near wrapping
504
505         double diff_ticks_corr_d =  (double)diff_ticks_corr;
506         double step_ticks = (m_dll_coeff_b * diff_ticks_corr_d);
507         debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE,
508                            "diff_ticks_corr=%f, step_ticks=%f\n",
509                            diff_ticks_corr_d, step_ticks);
510
511         // the same goes for m_dll_e2, which should be approx equal
512         // to the ticks/usec rate (= 24.576) hence also not near
513         // wrapping
514         step_ticks += m_dll_e2;
515         debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE,
516                            "add %f ticks to step_ticks => step_ticks=%f\n",
517                            m_dll_e2, step_ticks);
518
519         // it can't be that we have to update to a value in the past
520         if(step_ticks < 0) {
521             debugError("negative step: %f! (correcting to nominal)\n", step_ticks);
522             // recover to an estimated value
523             step_ticks = (double)m_ticks_per_update;
524         }
525
526         if(step_ticks > TICKS_PER_SECOND) {
527             debugWarning("rather large step: %f ticks (> 1sec)\n", step_ticks);
528         }
529
530         // now add the step ticks with wrapping.
531         m_next_time_ticks = (double)(addTicks((uint64_t)m_current_time_ticks, (uint64_t)step_ticks));
532
533         // update the DLL state
534         m_dll_e2 += m_dll_coeff_c * diff_ticks_corr_d;
535
536         // update the y-axis values
537         m_current_time_usecs = m_next_time_usecs;
538         m_next_time_usecs += m_usecs_per_update;
539
540         debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE,
541                            " usecs: current: %f next: %f usecs_late=%" PRId64 " ticks_late=%" PRId64 "\n",
542                            m_current_time_usecs, m_next_time_usecs, usecs_late, ticks_late);
543         debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE,
544                            " ticks: current: %f next: %f diff=%" PRId64 "\n",
545                            m_current_time_ticks, m_next_time_ticks, diff_ticks);
546         debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE,
547                            " ticks: current: %011" PRIu64 " (%03us %04ucy %04uticks)\n",
548                            (uint64_t)m_current_time_ticks,
549                            (unsigned int)TICKS_TO_SECS( (uint64_t)m_current_time_ticks ),
550                            (unsigned int)TICKS_TO_CYCLES( (uint64_t)m_current_time_ticks ),
551                            (unsigned int)TICKS_TO_OFFSET( (uint64_t)m_current_time_ticks ) );
552         debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE,
553                            " ticks: next   : %011" PRIu64 " (%03us %04ucy %04uticks)\n",
554                            (uint64_t)m_next_time_ticks,
555                            (unsigned int)TICKS_TO_SECS( (uint64_t)m_next_time_ticks ),
556                            (unsigned int)TICKS_TO_CYCLES( (uint64_t)m_next_time_ticks ),
557                            (unsigned int)TICKS_TO_OFFSET( (uint64_t)m_next_time_ticks ) );
558
559         debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE,
560                            " state: local: %11" PRIu64 ", dll_e2: %f, rate: %f\n",
561                            local_time, m_dll_e2, getRate());
562     }
563
564     // prepare the new compute vars
565     struct compute_vars new_vars;
566     new_vars.ticks = (uint64_t)(m_current_time_ticks);
567     new_vars.usecs = (uint64_t)m_current_time_usecs;
568     new_vars.rate = getRate();
569
570     // get the next index
571     unsigned int next_idx = (m_current_shadow_idx + 1) % CTRHELPER_NB_SHADOW_VARS;
572
573     // update the next index position
574     m_shadow_vars[next_idx] = new_vars;
575
576     // then we can update the current index
577     m_current_shadow_idx = next_idx;
578
579 #ifdef DEBUG
580     // do some verification
581     // we re-read a valid ctr timestamp
582     // then we use the attached system time to calculate
583     // the DLL generated timestamp and we check what the
584     // difference is
585
586     if(!readCycleTimerWithRetry(&cycle_timer, &local_time, 10)) {
587         debugError("Could not read cycle timer register (verify)\n");
588         return true; // true since this is a check only
589     }
590     cycle_timer_ticks = CYCLE_TIMER_TO_TICKS(cycle_timer);
591
592     // only check when successful
593     int64_t time_diff = local_time - new_vars.usecs;
594     double y_step_in_ticks = ((double)time_diff) * new_vars.rate;
595     int64_t y_step_in_ticks_int = (int64_t)y_step_in_ticks;
596     uint64_t offset_in_ticks_int = new_vars.ticks;
597     uint32_t dll_time;
598     if (y_step_in_ticks_int > 0) {
599         dll_time = addTicks(offset_in_ticks_int, y_step_in_ticks_int);
600     } else {
601         dll_time = substractTicks(offset_in_ticks_int, -y_step_in_ticks_int);
602     }
603     int32_t ctr_diff = cycle_timer_ticks-dll_time;
604     debugOutput(DEBUG_LEVEL_ULTRA_VERBOSE, "(%p) CTR DIFF: HW %010" PRIu64 " - DLL %010u = %010d (%s)\n",
605                 this, cycle_timer_ticks, dll_time, ctr_diff, (ctr_diff>0?"lag":"lead"));
606 #endif
607
608     return true;
609 }
610
611 uint32_t
612 CycleTimerHelper::getCycleTimerTicks()
613 {
614     uint64_t now = m_Parent.getCurrentTimeAsUsecs();
615     return getCycleTimerTicks(now);
616 }
617
618 uint32_t
619 CycleTimerHelper::getCycleTimerTicks(uint64_t now)
620 {
621     uint32_t retval;
622     struct compute_vars *my_vars;
623
624     // get pointer and copy the contents
625     // no locking should be needed since we have more than one
626     // of these vars available, and our use will always be finished before
627     // m_current_shadow_idx changes since this thread's priority should
628     // be higher than the one of the writer thread. Even if not, we only have to ensure
629     // that the used dataset is consistent. We can use an older dataset if it's consistent
630     // since it will also provide a fairly decent extrapolation.
631     my_vars = m_shadow_vars + m_current_shadow_idx;
632
633     int64_t time_diff = now - my_vars->usecs;
634     double y_step_in_ticks = ((double)time_diff) * my_vars->rate;
635     int64_t y_step_in_ticks_int = (int64_t)y_step_in_ticks;
636     uint64_t offset_in_ticks_int = my_vars->ticks;
637
638     if (y_step_in_ticks_int > 0) {
639         retval = addTicks(offset_in_ticks_int, y_step_in_ticks_int);
640 /*        debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE, "y_step_in_ticks_int > 0: %d, time_diff: %f, rate: %f, retval: %u\n",
641                     y_step_in_ticks_int, time_diff, my_vars.rate, retval);*/
642     } else {
643         retval = substractTicks(offset_in_ticks_int, -y_step_in_ticks_int);
644
645         // this can happen if the update thread was woken up earlier than it should have been
646 /*        debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE, "y_step_in_ticks_int <= 0: %d, time_diff: %f, rate: %f, retval: %u\n",
647                     y_step_in_ticks_int, time_diff, my_vars.rate, retval);*/
648     }
649
650     return retval;
651 }
652
653 uint32_t
654 CycleTimerHelper::getCycleTimer()
655 {
656     uint64_t now = m_Parent.getCurrentTimeAsUsecs();
657     return getCycleTimer(now);
658 }
659
660 uint32_t
661 CycleTimerHelper::getCycleTimer(uint64_t now)
662 {
663     uint32_t ticks = getCycleTimerTicks(now);
664     uint32_t result = TICKS_TO_CYCLE_TIMER(ticks);
665 #ifdef DEBUG
666     if(CYCLE_TIMER_TO_TICKS(result) != ticks) {
667         debugWarning("Bad ctr conversion");
668     }
669 #endif
670     return result;
671 }
672
673 uint64_t
674 CycleTimerHelper::getSystemTimeForCycleTimerTicks(uint32_t ticks)
675 {
676     uint64_t retval;
677     struct compute_vars *my_vars;
678
679     // get pointer and copy the contents
680     // no locking should be needed since we have more than one
681     // of these vars available, and our use will always be finished before
682     // m_current_shadow_idx changes since this thread's priority should
683     // be higher than the one of the writer thread. Even if not, we only have to ensure
684     // that the used dataset is consistent. We can use an older dataset if it's consistent
685     // since it will also provide a fairly decent extrapolation.
686     my_vars = m_shadow_vars + m_current_shadow_idx;
687
688     // the number of ticks the request is ahead of the current CTR position
689     int64_t ticks_diff = diffTicks(ticks, my_vars->ticks);
690     // to how much time does this correspond?
691     double x_step_in_usec = ((double)ticks_diff) / my_vars->rate;
692     int64_t x_step_in_usec_int = (int64_t)x_step_in_usec;
693     retval = my_vars->usecs + x_step_in_usec_int;
694
695     return retval;
696 }
697
698 uint64_t
699 CycleTimerHelper::getSystemTimeForCycleTimer(uint32_t ctr)
700 {
701     uint32_t ticks = CYCLE_TIMER_TO_TICKS(ctr);
702     return getSystemTimeForCycleTimerTicks(ticks);
703 }
704
705 #else
706
707 float
708 CycleTimerHelper::getRate()
709 {
710     return getNominalRate();
711 }
712
713 float
714 CycleTimerHelper::getNominalRate()
715 {
716     float rate = ((double)TICKS_PER_SECOND) / 1000000.0;
717     return rate;
718 }
719
720 bool
721 CycleTimerHelper::Execute()
722 {
723     usleep(1000*1000);
724     return true;
725 }
726
727 uint32_t
728 CycleTimerHelper::getCycleTimerTicks()
729 {
730     return CYCLE_TIMER_TO_TICKS(getCycleTimer());
731 }
732
733 uint32_t
734 CycleTimerHelper::getCycleTimerTicks(uint64_t now)
735 {
736     debugWarning("untested code\n");
737     #warning Untested code
738     uint32_t cycle_timer;
739     uint64_t local_time;
740     readCycleTimerWithRetry(&cycle_timer, &local_time, 10);
741     int64_t ticks = CYCLE_TIMER_TO_TICKS(cycle_timer);
742
743     int delta_t = now - local_time; // how far ahead is the request?
744     ticks += delta_t * getRate(); // add ticks
745     if (ticks >= TICKS_PER_SECOND * 128) ticks -= TICKS_PER_SECOND * 128;
746     else if (ticks < 0) ticks += TICKS_PER_SECOND * 128;
747     return ticks;
748 }
749
750 uint32_t
751 CycleTimerHelper::getCycleTimer()
752 {
753     uint32_t cycle_timer;
754     uint64_t local_time;
755     readCycleTimerWithRetry(&cycle_timer, &local_time, 10);
756     return cycle_timer;
757 }
758
759 uint32_t
760 CycleTimerHelper::getCycleTimer(uint64_t now)
761 {
762     return TICKS_TO_CYCLE_TIMER(getCycleTimerTicks(now));
763 }
764
765 uint64_t
766 CycleTimerHelper::getSystemTimeForCycleTimerTicks(uint32_t ticks)
767 {
768     debugWarning("not implemented!\n");
769     return 0;
770 }
771
772 uint64_t
773 CycleTimerHelper::getSystemTimeForCycleTimer(uint32_t ctr)
774 {
775     uint32_t ticks = CYCLE_TIMER_TO_TICKS(ctr);
776     return getSystemTimeForCycleTimerTicks(ticks);
777 }
778
779 #endif
780
781 bool
782 CycleTimerHelper::readCycleTimerWithRetry(uint32_t *cycle_timer, uint64_t *local_time, int ntries)
783 {
784     bool good=false;
785     int maxtries = ntries;
786
787     do {
788         // the ctr read can return 0 sometimes. if that happens, reread the ctr.
789         int maxtries2=ntries;
790         do {
791             if(!m_Parent.readCycleTimerReg(cycle_timer, local_time)) {
792                 debugError("Could not read cycle timer register\n");
793                 return false;
794             }
795             if (*cycle_timer == 0) {
796                 debugOutput(DEBUG_LEVEL_VERBOSE,
797                            "Bogus CTR: %08X on try %02d\n",
798                            *cycle_timer, maxtries2);
799             }
800         } while (*cycle_timer == 0 && maxtries2--);
801        
802         // catch bogus ctr reads (can happen)
803         uint64_t cycle_timer_ticks = CYCLE_TIMER_TO_TICKS(*cycle_timer);
804    
805         if (diffTicks(cycle_timer_ticks, m_cycle_timer_ticks_prev) < 0) {
806             debugOutput( DEBUG_LEVEL_VERY_VERBOSE,
807                         "non-monotonic CTR (try %02d): %" PRIu64 " -> %" PRIu64 "\n",
808                         maxtries, m_cycle_timer_ticks_prev, cycle_timer_ticks);
809             debugOutput( DEBUG_LEVEL_VERY_VERBOSE,
810                         "                            : %08X -> %08X\n",
811                         m_cycle_timer_prev, *cycle_timer);
812             debugOutput( DEBUG_LEVEL_VERY_VERBOSE,
813                         " current: %011" PRIu64 " (%03us %04ucy %04uticks)\n",
814                         cycle_timer_ticks,
815                         (unsigned int)TICKS_TO_SECS( cycle_timer_ticks ),
816                         (unsigned int)TICKS_TO_CYCLES( cycle_timer_ticks ),
817                         (unsigned int)TICKS_TO_OFFSET( cycle_timer_ticks ) );
818             debugOutput( DEBUG_LEVEL_VERY_VERBOSE,
819                         " prev   : %011" PRIu64 " (%03us %04ucy %04uticks)\n",
820                         m_cycle_timer_ticks_prev,
821                         (unsigned int)TICKS_TO_SECS( m_cycle_timer_ticks_prev ),
822                         (unsigned int)TICKS_TO_CYCLES( m_cycle_timer_ticks_prev ),
823                         (unsigned int)TICKS_TO_OFFSET( m_cycle_timer_ticks_prev ) );
824         } else {
825             good = true;
826             m_cycle_timer_prev = *cycle_timer;
827             m_cycle_timer_ticks_prev = cycle_timer_ticks;
828         }
829     } while (!good && maxtries--);
830     return true;
831 }
832
833 void
834 CycleTimerHelper::setVerboseLevel(int l)
835 {
836     setDebugLevel(l);
837 }
Note: See TracBrowser for help on using the browser.