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

Revision 879, 15.6 kB (checked in by ppalmers, 16 years ago)

- improve cycle timer DLL

  • make it robust against bogus CTR reads
  • remove offset introduced by delay between wakeup and CTR read

prediction performance is now around 100 ticks for a 50ms update loop.

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
30 #define DLL_BANDWIDTH (0.1)
31 #define DLL_PI        (3.141592653589793238)
32 #define DLL_SQRT2     (1.414213562373095049)
33 #define DLL_OMEGA     (2.0*DLL_PI*DLL_BANDWIDTH)
34 #define DLL_COEFF_B   (DLL_SQRT2 * DLL_OMEGA)
35 #define DLL_COEFF_C   (DLL_OMEGA * DLL_OMEGA)
36
37 #define OFFSET_AVERAGE_COEFF 0.01
38 /*
39 #define ENTER_CRITICAL_SECTION { \
40     if (pthread_mutex_trylock(&m_compute_vars_lock) == EBUSY) { \
41         debugWarning(" (%p) lock clash\n", this); \
42         ENTER_CRITICAL_SECTION; \
43     } \
44     }
45 */
46 #define ENTER_CRITICAL_SECTION { \
47     pthread_mutex_lock(&m_compute_vars_lock); \
48     }
49 #define EXIT_CRITICAL_SECTION { \
50     pthread_mutex_unlock(&m_compute_vars_lock); \
51     }
52
53 IMPL_DEBUG_MODULE( CycleTimerHelper, CycleTimerHelper, DEBUG_LEVEL_NORMAL );
54
55 CycleTimerHelper::CycleTimerHelper(Ieee1394Service &parent, unsigned int update_period_us)
56     : m_Parent ( parent )
57     , m_ticks_per_update ( ((uint64_t)TICKS_PER_SECOND) * ((uint64_t)update_period_us) / 1000000ULL )
58     , m_usecs_per_update ( update_period_us )
59     , m_avg_wakeup_delay ( 0.0 )
60     , m_dll_e2 ( 0.0 )
61     , m_current_time_usecs ( 0 )
62     , m_next_time_usecs ( 0 )
63     , m_current_time_ticks ( 0 )
64     , m_next_time_ticks ( 0 )
65     , m_first_run ( true )
66     , m_sleep_until ( 0 )
67     , m_cycle_timer_prev ( 0 )
68     , m_cycle_timer_ticks_prev ( 0 )
69     , m_Thread ( NULL )
70     , m_realtime ( false )
71     , m_priority ( 0 )
72 {
73     debugOutput( DEBUG_LEVEL_VERBOSE, "Create %p...\n", this);
74 }
75
76 CycleTimerHelper::CycleTimerHelper(Ieee1394Service &parent, unsigned int update_period_us, bool rt, int prio)
77     : m_Parent ( parent )
78     , m_ticks_per_update ( ((uint64_t)TICKS_PER_SECOND) * ((uint64_t)update_period_us) / 1000000ULL )
79     , m_usecs_per_update ( update_period_us )
80     , m_avg_wakeup_delay ( 0.0 )
81     , m_dll_e2 ( 0.0 )
82     , m_current_time_usecs ( 0 )
83     , m_next_time_usecs ( 0 )
84     , m_current_time_ticks ( 0 )
85     , m_next_time_ticks ( 0 )
86     , m_first_run ( true )
87     , m_sleep_until ( 0 )
88     , m_cycle_timer_prev ( 0 )
89     , m_cycle_timer_ticks_prev ( 0 )
90     , m_Thread ( NULL )
91     , m_realtime ( rt )
92     , m_priority ( prio )
93 {
94     debugOutput( DEBUG_LEVEL_VERBOSE, "Create %p...\n", this);
95 }
96
97 CycleTimerHelper::~CycleTimerHelper()
98 {
99     if (m_Thread) {
100         m_Thread->Stop();
101         delete m_Thread;
102     }
103 }
104
105 bool
106 CycleTimerHelper::Start()
107 {
108     debugOutput( DEBUG_LEVEL_VERBOSE, "Start %p...\n", this);
109 #if IEEE1394SERVICE_USE_CYCLETIMER_DLL
110     m_Thread = new Util::PosixThread(this, m_realtime, m_priority,
111                                      PTHREAD_CANCEL_DEFERRED);
112     if(!m_Thread) {
113         debugFatal("No thread\n");
114         return false;
115     }
116     if (m_Thread->Start() != 0) {
117         debugFatal("Could not start update thread\n");
118         return false;
119     }
120 #endif
121     return true;
122 }
123
124 bool
125 CycleTimerHelper::Init()
126 {
127     debugOutput( DEBUG_LEVEL_VERBOSE, "Initialize %p...\n", this);
128     pthread_mutex_init(&m_compute_vars_lock, NULL);
129    
130     // initialize the 'prev ctr' values
131     uint64_t local_time;
132     int maxtries2 = 10;
133     do {
134         if(!m_Parent.readCycleTimerReg(&m_cycle_timer_prev, &local_time)) {
135             debugError("Could not read cycle timer register\n");
136             return false;
137         }
138         if (m_cycle_timer_prev == 0) {
139             debugOutput(DEBUG_LEVEL_VERBOSE,
140                         "Bogus CTR: %08X on try %02d\n",
141                         m_cycle_timer_prev, maxtries2);
142         }
143     } while (m_cycle_timer_prev == 0 && maxtries2--);
144     m_cycle_timer_ticks_prev = CYCLE_TIMER_TO_TICKS(m_cycle_timer_prev);
145     return true;
146 }
147
148 bool
149 CycleTimerHelper::setThreadParameters(bool rt, int priority) {
150     debugOutput( DEBUG_LEVEL_VERBOSE, "(%p) switch to: (rt=%d, prio=%d)...\n", this, rt, priority);
151     if (priority > THREAD_MAX_RTPRIO) priority = THREAD_MAX_RTPRIO; // cap the priority
152     m_realtime = rt;
153     m_priority = priority;
154
155 #if IEEE1394SERVICE_USE_CYCLETIMER_DLL
156     if (m_Thread) {
157         if (m_realtime) {
158             m_Thread->AcquireRealTime(m_priority);
159         } else {
160             m_Thread->DropRealTime();
161         }
162     }
163 #endif
164
165     return true;
166 }
167
168 #if IEEE1394SERVICE_USE_CYCLETIMER_DLL
169 float
170 CycleTimerHelper::getRate()
171 {
172     float rate = (float)(diffTicks((uint64_t)m_next_time_ticks, (uint64_t)m_current_time_ticks));
173     rate /= (float)(m_next_time_usecs - m_current_time_usecs);
174     return rate;
175 }
176
177 float
178 CycleTimerHelper::getNominalRate()
179 {
180     float rate = ((double)TICKS_PER_SECOND) / 1000000.0;
181     return rate;
182 }
183
184 bool
185 CycleTimerHelper::Execute()
186 {
187     debugOutputExtreme( DEBUG_LEVEL_VERY_VERBOSE, "Execute %p...\n", this);
188     if (!m_first_run) {
189         // wait for the next update period
190         ffado_microsecs_t now = m_TimeSource.getCurrentTimeAsUsecs();
191         int sleep_time = m_sleep_until - now;
192         debugOutputExtreme( DEBUG_LEVEL_VERY_VERBOSE, "(%p) Sleep until %lld/%f (now: %lld, diff=%d) ...\n",
193                     this, m_sleep_until, m_next_time_usecs, now, sleep_time);
194         m_TimeSource.SleepUsecAbsolute(m_sleep_until);
195         debugOutputExtreme( DEBUG_LEVEL_VERY_VERBOSE, " (%p) back...\n", this);
196     }
197
198     uint32_t cycle_timer;
199     uint64_t local_time;
200     int64_t usecs_late;
201     int ntries=2;
202     uint64_t cycle_timer_ticks;
203     double diff_ticks;
204
205     // if the difference between the predicted value and the
206     // actual value seems to be too large, retry reading the cycle timer
207     // some host controllers return bogus values on some reads
208     // (looks like a non-atomic update of the register)
209     do {
210         if(!readCycleTimerWithRetry(&cycle_timer, &local_time, 10)) {
211             debugError("Could not read cycle timer register\n");
212             return false;
213         }
214         usecs_late = local_time - m_sleep_until;
215         cycle_timer_ticks = CYCLE_TIMER_TO_TICKS(cycle_timer);
216         diff_ticks = diffTicks(cycle_timer_ticks, (int64_t)m_next_time_ticks);
217         if(diff_ticks < -((double)TICKS_PER_HALFCYCLE)) {
218             debugOutput(DEBUG_LEVEL_VERBOSE, "have to retry, diff = %f\n",diff_ticks);
219         }
220        
221     } while(diff_ticks < -((double)TICKS_PER_HALFCYCLE) && --ntries && !m_first_run);
222
223     debugOutputExtreme( DEBUG_LEVEL_VERY_VERBOSE, " read : CTR: %11lu, local: %17llu\n",
224                         cycle_timer, local_time);
225
226     if (m_first_run) {
227         m_sleep_until = local_time + m_usecs_per_update;
228         m_dll_e2 = m_ticks_per_update;
229         m_current_time_usecs = local_time;
230         m_next_time_usecs = m_current_time_usecs + m_usecs_per_update;
231         m_current_time_ticks = CYCLE_TIMER_TO_TICKS( cycle_timer );
232         m_next_time_ticks = addTicks( (uint64_t)m_current_time_ticks, (uint64_t)m_dll_e2);
233         debugOutput( DEBUG_LEVEL_VERBOSE, " First run\n");
234         debugOutput( DEBUG_LEVEL_VERBOSE, "  usecs/update: %lu, ticks/update: %lu, m_dll_e2: %f\n",
235                                           m_usecs_per_update, m_ticks_per_update, m_dll_e2);
236         debugOutput( DEBUG_LEVEL_VERBOSE, "  usecs current: %f, next: %f\n", m_current_time_usecs, m_next_time_usecs);
237         debugOutput( DEBUG_LEVEL_VERBOSE, "  ticks current: %f, next: %f\n", m_current_time_ticks, m_next_time_ticks);
238         m_first_run = false;
239     } else {
240         m_sleep_until += m_usecs_per_update;
241
242         double diff_ticks_corr;
243         // correct for late wakeup
244         int64_t ticks_late = (usecs_late * TICKS_PER_SECOND) / 1000000LL;
245         if (ticks_late > 0) {
246             // if we are usecs_late usecs late
247             // the cycle timer has ticked approx ticks_late ticks too much
248             cycle_timer_ticks = substractTicks(cycle_timer_ticks, ticks_late);
249             diff_ticks_corr = diff_ticks - ticks_late;
250         } else {
251             debugError("Early wakeup, should not happen!\n");
252             // recover
253             cycle_timer_ticks = addTicks(cycle_timer_ticks, -ticks_late);
254             diff_ticks_corr = diff_ticks + ticks_late;
255         }
256
257         #ifdef DEBUG
258         if(usecs_late > 20) {
259             debugWarning("Rather late wakeup: %lld usecs\n", usecs_late);
260         }
261         #endif
262
263         // update the x-axis values
264         m_current_time_ticks = m_next_time_ticks;
265
266         // do the calculation in 'tick space'
267         int64_t tmp = (uint64_t)(DLL_COEFF_B * diff_ticks_corr);
268         if(m_dll_e2 > 0) {
269             tmp = addTicks(tmp, (uint64_t)m_dll_e2);
270         } else {
271             tmp = substractTicks(tmp, (uint64_t)(-m_dll_e2));
272         }
273         if(tmp < 0) {
274             debugWarning("negative slope: %lld!\n", tmp);
275         }
276         m_next_time_ticks = addTicks((uint64_t)m_current_time_ticks, tmp);
277
278         // it should be ok to not do this in tick space since it's value
279         // is approx equal to the rate, being 24.576 ticks/usec
280         m_dll_e2 += DLL_COEFF_C * diff_ticks_corr;
281
282         // For jitter graphs
283         //debugOutputShort(DEBUG_LEVEL_NORMAL, "0123456789 %f %f %f %lld %lld %lld\n",
284         //                 diff_ticks, diff_ticks_corr, m_dll_e2, cycle_timer_ticks, (int64_t)m_next_time_ticks, usecs_late);
285
286         // update the y-axis values
287         m_current_time_usecs = m_next_time_usecs;
288         m_next_time_usecs += m_usecs_per_update;
289
290         debugOutputExtreme( DEBUG_LEVEL_VERY_VERBOSE, " usecs: current: %f next: %f usecs_late=%lld ticks_late=%lld\n",
291                             m_current_time_usecs, m_next_time_usecs, usecs_late, ticks_late);
292         debugOutputExtreme( DEBUG_LEVEL_VERY_VERBOSE, " ticks: current: %f next: %f diff=%f\n",
293                             m_current_time_ticks, m_next_time_ticks, diff_ticks);
294         debugOutputExtreme( DEBUG_LEVEL_VERY_VERBOSE, " state: local: %11llu, dll_e2: %f, rate: %f\n",
295                             local_time, m_dll_e2, getRate());
296     }
297
298     // FIXME: priority inversion possible, run this at higher prio than client threads
299     ENTER_CRITICAL_SECTION;
300     m_current_vars.ticks = (uint64_t)(m_current_time_ticks);
301     m_current_vars.usecs = (uint64_t)m_current_time_usecs;
302     m_current_vars.rate = getRate();
303     EXIT_CRITICAL_SECTION;
304
305     return true;
306 }
307
308 uint32_t
309 CycleTimerHelper::getCycleTimerTicks()
310 {
311     uint64_t now = m_Parent.getCurrentTimeAsUsecs();
312     return getCycleTimerTicks(now);
313 }
314
315 uint32_t
316 CycleTimerHelper::getCycleTimerTicks(uint64_t now)
317 {
318     uint32_t retval;
319     struct compute_vars my_vars;
320
321     // reduce lock contention
322     ENTER_CRITICAL_SECTION;
323     my_vars = m_current_vars;
324     EXIT_CRITICAL_SECTION;
325
326     int64_t time_diff = now - my_vars.usecs;
327     double y_step_in_ticks = ((double)time_diff) * my_vars.rate;
328     int64_t y_step_in_ticks_int = (int64_t)y_step_in_ticks;
329     uint64_t offset_in_ticks_int = my_vars.ticks;
330
331     if (y_step_in_ticks_int > 0) {
332         retval = addTicks(offset_in_ticks_int, y_step_in_ticks_int);
333         debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE, "y_step_in_ticks_int > 0: %lld, time_diff: %f, rate: %f, retval: %lu\n",
334                     y_step_in_ticks_int, time_diff, my_vars.rate, retval);
335     } else {
336         retval = substractTicks(offset_in_ticks_int, -y_step_in_ticks_int);
337
338         // this can happen if the update thread was woken up earlier than it should have been
339         debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE, "y_step_in_ticks_int <= 0: %lld, time_diff: %f, rate: %f, retval: %lu\n",
340                     y_step_in_ticks_int, time_diff, my_vars.rate, retval);
341     }
342
343     return retval;
344 }
345
346 uint32_t
347 CycleTimerHelper::getCycleTimer()
348 {
349     uint64_t now = m_Parent.getCurrentTimeAsUsecs();
350     return getCycleTimer(now);
351 }
352
353 uint32_t
354 CycleTimerHelper::getCycleTimer(uint64_t now)
355 {
356     uint32_t ticks = getCycleTimerTicks(now);
357     uint32_t result = TICKS_TO_CYCLE_TIMER(ticks);
358 #ifdef DEBUG
359     if(CYCLE_TIMER_TO_TICKS(result) != ticks) {
360         debugWarning("Bad ctr conversion");
361     }
362 #endif
363     return result;
364 }
365
366 #else
367
368 float
369 CycleTimerHelper::getRate()
370 {
371     return getNominalRate();
372 }
373
374 float
375 CycleTimerHelper::getNominalRate()
376 {
377     float rate = ((double)TICKS_PER_SECOND) / 1000000.0;
378     return rate;
379 }
380
381 bool
382 CycleTimerHelper::Execute()
383 {
384     usleep(1000*1000);
385     return true;
386 }
387
388 uint32_t
389 CycleTimerHelper::getCycleTimerTicks()
390 {
391     return CYCLE_TIMER_TO_TICKS(getCycleTimer());
392 }
393
394 uint32_t
395 CycleTimerHelper::getCycleTimerTicks(uint64_t now)
396 {
397     debugWarning("not implemented!\n");
398     return getCycleTimerTicks();
399 }
400
401 uint32_t
402 CycleTimerHelper::getCycleTimer()
403 {
404     uint32_t cycle_timer;
405     uint64_t local_time;
406     readCycleTimerWithRetry(&cycle_timer, &local_time, 10);
407     return cycle_timer;
408 }
409
410 uint32_t
411 CycleTimerHelper::getCycleTimer(uint64_t now)
412 {
413     debugWarning("not implemented!\n");
414     return getCycleTimer();
415 }
416
417 #endif
418
419 bool
420 CycleTimerHelper::readCycleTimerWithRetry(uint32_t *cycle_timer, uint64_t *local_time, int ntries)
421 {
422     bool good=false;
423     int maxtries = ntries;
424
425     do {
426         // the ctr read can return 0 sometimes. if that happens, reread the ctr.
427         int maxtries2=ntries;
428         do {
429             if(!m_Parent.readCycleTimerReg(cycle_timer, local_time)) {
430                 debugError("Could not read cycle timer register\n");
431                 return false;
432             }
433             if (*cycle_timer == 0) {
434                 debugOutput(DEBUG_LEVEL_VERBOSE,
435                            "Bogus CTR: %08X on try %02d\n",
436                            *cycle_timer, maxtries2);
437             }
438         } while (*cycle_timer == 0 && maxtries2--);
439        
440         // catch bogus ctr reads (can happen)
441         uint64_t cycle_timer_ticks = CYCLE_TIMER_TO_TICKS(*cycle_timer);
442    
443         if (diffTicks(cycle_timer_ticks, m_cycle_timer_ticks_prev) < 0) {
444             debugOutput( DEBUG_LEVEL_VERBOSE,
445                         "non-monotonic CTR (try %02d): %llu -> %llu\n",
446                         maxtries, m_cycle_timer_ticks_prev, cycle_timer_ticks);
447             debugOutput( DEBUG_LEVEL_VERBOSE,
448                         "                            : %08X -> %08X\n",
449                         m_cycle_timer_prev, *cycle_timer);
450             debugOutput( DEBUG_LEVEL_VERBOSE,
451                         " current: %011llu (%03us %04ucy %04uticks)\n",
452                         cycle_timer_ticks,
453                         (unsigned int)TICKS_TO_SECS( cycle_timer_ticks ),
454                         (unsigned int)TICKS_TO_CYCLES( cycle_timer_ticks ),
455                         (unsigned int)TICKS_TO_OFFSET( cycle_timer_ticks ) );
456             debugOutput( DEBUG_LEVEL_VERBOSE,
457                         " prev   : %011llu (%03us %04ucy %04uticks)\n",
458                         m_cycle_timer_ticks_prev,
459                         (unsigned int)TICKS_TO_SECS( m_cycle_timer_ticks_prev ),
460                         (unsigned int)TICKS_TO_CYCLES( m_cycle_timer_ticks_prev ),
461                         (unsigned int)TICKS_TO_OFFSET( m_cycle_timer_ticks_prev ) );
462         } else {
463             good = true;
464             m_cycle_timer_prev = *cycle_timer;
465             m_cycle_timer_ticks_prev = cycle_timer_ticks;
466         }
467     } while (!good && maxtries--);
468     return true;
469 }
470
471 void
472 CycleTimerHelper::setVerboseLevel(int l)
473 {
474     setDebugLevel(l);
475 }
Note: See TracBrowser for help on using the browser.