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

Revision 882, 16.6 kB (checked in by ppalmers, 16 years ago)

use more sane value for late wakeup detection

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