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

Revision 878, 13.2 kB (checked in by ppalmers, 13 years ago)

fix nonmonotonic CTR reads (occurs on NEC card)

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     return true;
130 }
131
132 bool
133 CycleTimerHelper::setThreadParameters(bool rt, int priority) {
134     debugOutput( DEBUG_LEVEL_VERBOSE, "(%p) switch to: (rt=%d, prio=%d)...\n", this, rt, priority);
135     if (priority > THREAD_MAX_RTPRIO) priority = THREAD_MAX_RTPRIO; // cap the priority
136     m_realtime = rt;
137     m_priority = priority;
138
139 #if IEEE1394SERVICE_USE_CYCLETIMER_DLL
140     if (m_Thread) {
141         if (m_realtime) {
142             m_Thread->AcquireRealTime(m_priority);
143         } else {
144             m_Thread->DropRealTime();
145         }
146     }
147 #endif
148
149     return true;
150 }
151
152 #if IEEE1394SERVICE_USE_CYCLETIMER_DLL
153 float
154 CycleTimerHelper::getRate()
155 {
156     float rate = (float)(diffTicks((uint64_t)m_next_time_ticks, (uint64_t)m_current_time_ticks));
157     rate /= (float)(m_next_time_usecs - m_current_time_usecs);
158     return rate;
159 }
160
161 float
162 CycleTimerHelper::getNominalRate()
163 {
164     float rate = ((double)TICKS_PER_SECOND) / 1000000.0;
165     return rate;
166 }
167
168 bool
169 CycleTimerHelper::Execute()
170 {
171     debugOutputExtreme( DEBUG_LEVEL_VERY_VERBOSE, "Execute %p...\n", this);
172     if (!m_first_run) {
173         // wait for the next update period
174         ffado_microsecs_t now = m_TimeSource.getCurrentTimeAsUsecs();
175         int sleep_time = m_sleep_until - now;
176         debugOutput( DEBUG_LEVEL_VERY_VERBOSE, "(%p) Sleep until %lld/%f (now: %lld, diff=%d) ...\n",
177                     this, m_sleep_until, m_next_time_usecs, now, sleep_time);
178         m_TimeSource.SleepUsecAbsolute(m_sleep_until);
179         debugOutput( DEBUG_LEVEL_VERY_VERBOSE, " (%p) back...\n", this);
180     }
181
182     uint32_t cycle_timer;
183     uint64_t local_time;
184     if(!m_Parent.readCycleTimerReg(&cycle_timer, &local_time)) {
185         debugError("Could not read cycle timer register\n");
186         return false;
187     }
188     debugOutputExtreme( DEBUG_LEVEL_VERY_VERBOSE, " read : CTR: %11lu, local: %17llu\n",
189                         cycle_timer, local_time);
190
191     if (m_first_run) {
192         m_sleep_until = local_time + m_usecs_per_update;
193         m_dll_e2 = m_ticks_per_update;
194         m_current_time_usecs = local_time;
195         m_next_time_usecs = m_current_time_usecs + m_usecs_per_update;
196         m_current_time_ticks = CYCLE_TIMER_TO_TICKS( cycle_timer );
197         m_next_time_ticks = addTicks( (uint64_t)m_current_time_ticks, (uint64_t)m_dll_e2);
198         debugOutput( DEBUG_LEVEL_VERBOSE, " First run\n");
199         debugOutput( DEBUG_LEVEL_VERBOSE, "  usecs/update: %lu, ticks/update: %lu, m_dll_e2: %f\n",
200                                           m_usecs_per_update, m_ticks_per_update, m_dll_e2);
201         debugOutput( DEBUG_LEVEL_VERBOSE, "  usecs current: %f, next: %f\n", m_current_time_usecs, m_next_time_usecs);
202         debugOutput( DEBUG_LEVEL_VERBOSE, "  ticks current: %f, next: %f\n", m_current_time_ticks, m_next_time_ticks);
203         m_first_run = false;
204     } else {
205         m_sleep_until += m_usecs_per_update;
206         uint64_t cycle_timer_ticks = CYCLE_TIMER_TO_TICKS(cycle_timer);
207         double usecs_late = ((double)local_time) - (m_next_time_usecs);
208
209         // update the x-axis values
210         double diff_ticks = diffTicks(cycle_timer_ticks, (int64_t)m_next_time_ticks);
211         m_current_time_ticks = m_next_time_ticks;
212         // do the calculation in 'tick space'
213         int64_t tmp = (uint64_t)(DLL_COEFF_B * diff_ticks);
214         if(m_dll_e2 > 0) {
215             tmp = addTicks(tmp, (uint64_t)m_dll_e2);
216         } else {
217             tmp = substractTicks(tmp, (uint64_t)(-m_dll_e2));
218         }
219         if(tmp < 0) {
220             debugWarning("negative slope: %lld!\n", tmp);
221         }
222         m_next_time_ticks = addTicks((uint64_t)m_current_time_ticks, tmp);
223
224         // it should be ok to not do this in tick space since it's value
225         // is approx equal to the rate, being 24.576 ticks/usec
226         m_dll_e2 += DLL_COEFF_C * diff_ticks;
227
228         // update the y-axis values
229         m_current_time_usecs = m_next_time_usecs;
230         m_next_time_usecs = local_time + m_usecs_per_update;
231
232         debugOutputExtreme( DEBUG_LEVEL_VERY_VERBOSE, " usecs: current: %f next: %f usecs_late=%f\n",
233                             m_current_time_usecs, m_next_time_usecs, usecs_late);
234         debugOutputExtreme( DEBUG_LEVEL_VERY_VERBOSE, " ticks: current: %f next: %f diff=%f\n",
235                             m_current_time_ticks, m_next_time_ticks, diff_ticks);
236         debugOutputExtreme( DEBUG_LEVEL_VERY_VERBOSE, " state: local: %11llu, dll_e2: %f, rate: %f\n",
237                             local_time, m_dll_e2, getRate());
238     }
239
240     // FIXME: priority inversion possible, run this at higher prio than client threads
241     ENTER_CRITICAL_SECTION;
242     m_current_vars.ticks = (uint64_t)(m_current_time_ticks);// + m_average_offset_ticks);
243     m_current_vars.usecs = (uint64_t)m_current_time_usecs;
244     m_current_vars.rate = getRate();
245     EXIT_CRITICAL_SECTION;
246
247     return true;
248 }
249
250 uint32_t
251 CycleTimerHelper::getCycleTimerTicks()
252 {
253     uint64_t now = m_Parent.getCurrentTimeAsUsecs();
254     return getCycleTimerTicks(now);
255 }
256
257 uint32_t
258 CycleTimerHelper::getCycleTimerTicks(uint64_t now)
259 {
260     uint32_t retval;
261     struct compute_vars my_vars;
262
263     // reduce lock contention
264     ENTER_CRITICAL_SECTION;
265     my_vars = m_current_vars;
266     EXIT_CRITICAL_SECTION;
267
268     int64_t time_diff = now - my_vars.usecs;
269     double y_step_in_ticks = ((double)time_diff) * my_vars.rate;
270     int64_t y_step_in_ticks_int = (int64_t)y_step_in_ticks;
271     uint64_t offset_in_ticks_int = my_vars.ticks;
272
273     if (y_step_in_ticks_int > 0) {
274         retval = addTicks(offset_in_ticks_int, y_step_in_ticks_int);
275         debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE, "y_step_in_ticks_int > 0: %lld, time_diff: %f, rate: %f, retval: %lu\n",
276                     y_step_in_ticks_int, time_diff, my_vars.rate, retval);
277     } else {
278         retval = substractTicks(offset_in_ticks_int, -y_step_in_ticks_int);
279
280         // this can happen if the update thread was woken up earlier than it should have been
281         debugOutputExtreme(DEBUG_LEVEL_VERY_VERBOSE, "y_step_in_ticks_int <= 0: %lld, time_diff: %f, rate: %f, retval: %lu\n",
282                     y_step_in_ticks_int, time_diff, my_vars.rate, retval);
283     }
284
285     return retval;
286 }
287
288 uint32_t
289 CycleTimerHelper::getCycleTimer()
290 {
291     uint64_t now = m_Parent.getCurrentTimeAsUsecs();
292     return getCycleTimer(now);
293 }
294
295 uint32_t
296 CycleTimerHelper::getCycleTimer(uint64_t now)
297 {
298     uint32_t ticks = getCycleTimerTicks(now);
299     uint32_t result = TICKS_TO_CYCLE_TIMER(ticks);
300 #ifdef DEBUG
301     if(CYCLE_TIMER_TO_TICKS(result) != ticks) {
302         debugWarning("Bad ctr conversion");
303     }
304 #endif
305     return result;
306 }
307
308 #else
309
310 float
311 CycleTimerHelper::getRate()
312 {
313     return getNominalRate();
314 }
315
316 float
317 CycleTimerHelper::getNominalRate()
318 {
319     float rate = ((double)TICKS_PER_SECOND) / 1000000.0;
320     return rate;
321 }
322
323 bool
324 CycleTimerHelper::Execute()
325 {
326     usleep(1000*1000);
327     return true;
328 }
329
330 uint32_t
331 CycleTimerHelper::getCycleTimerTicks()
332 {
333     return CYCLE_TIMER_TO_TICKS(getCycleTimer());
334 }
335
336 uint32_t
337 CycleTimerHelper::getCycleTimerTicks(uint64_t now)
338 {
339     debugWarning("not implemented!\n");
340     return getCycleTimerTicks();
341 }
342
343 uint32_t
344 CycleTimerHelper::getCycleTimer()
345 {
346     bool good=false;
347     int maxtries = 10;
348     uint32_t cycle_timer;
349     uint64_t local_time;
350    
351     do {
352         // the ctr read can return 0 sometimes. if that happens, reread the ctr.
353         int maxtries2=10;
354         do {
355             if(!m_Parent.readCycleTimerReg(&cycle_timer, &local_time)) {
356                 debugError("Could not read cycle timer register\n");
357                 return 0;
358             }
359             if (cycle_timer == 0) {
360                 debugOutput(DEBUG_LEVEL_VERBOSE,
361                            "Bogus CTR: %08X on try %02d\n",
362                            cycle_timer, maxtries2);
363             }
364         } while (cycle_timer == 0 && maxtries2--);
365        
366         // catch bogus ctr reads (can happen)
367         uint64_t cycle_timer_ticks = CYCLE_TIMER_TO_TICKS(cycle_timer);
368    
369         if (diffTicks(cycle_timer_ticks, m_cycle_timer_ticks_prev) < 0) {
370             debugOutput( DEBUG_LEVEL_VERBOSE,
371                         "non-monotonic CTR (try %02d): %llu -> %llu\n",
372                         maxtries, m_cycle_timer_ticks_prev, cycle_timer_ticks);
373             debugOutput( DEBUG_LEVEL_VERBOSE,
374                         "                            : %08X -> %08X\n",
375                         m_cycle_timer_prev, cycle_timer);
376             debugOutput( DEBUG_LEVEL_VERBOSE,
377                         " current: %011llu (%03us %04ucy %04uticks)\n",
378                         cycle_timer_ticks,
379                         (unsigned int)TICKS_TO_SECS( cycle_timer_ticks ),
380                         (unsigned int)TICKS_TO_CYCLES( cycle_timer_ticks ),
381                         (unsigned int)TICKS_TO_OFFSET( cycle_timer_ticks ) );
382             debugOutput( DEBUG_LEVEL_VERBOSE,
383                         " prev   : %011llu (%03us %04ucy %04uticks)\n",
384                         m_cycle_timer_ticks_prev,
385                         (unsigned int)TICKS_TO_SECS( m_cycle_timer_ticks_prev ),
386                         (unsigned int)TICKS_TO_CYCLES( m_cycle_timer_ticks_prev ),
387                         (unsigned int)TICKS_TO_OFFSET( m_cycle_timer_ticks_prev ) );
388         } else {
389             good = true;
390             m_cycle_timer_prev = cycle_timer;
391             m_cycle_timer_ticks_prev = cycle_timer_ticks;
392         }
393     } while (!good && maxtries--);
394     return cycle_timer;
395 }
396
397 uint32_t
398 CycleTimerHelper::getCycleTimer(uint64_t now)
399 {
400     debugWarning("not implemented!\n");
401     return getCycleTimer();
402 }
403
404 #endif
405
406 void
407 CycleTimerHelper::setVerboseLevel(int l)
408 {
409     setDebugLevel(l);
410 }
Note: See TracBrowser for help on using the browser.