root/branches/libffado-2.0/src/libieee1394/CycleTimerHelper.cpp

Revision 1544, 29.9 kB (checked in by ppalmers, 12 years ago)

round the transmit safety buffer size to one packet size (in frames) to avoid messing up the MIDI time muxed stream position

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