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

Revision 1498, 30.0 kB (checked in by ppalmers, 15 years ago)

Merge all changes from 2.0 branch into trunk (since r1361). This _should_ contain all forward merges done in the mean time. At this moment in time both branches should be in sync.

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