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

Revision 807, 10.3 kB (checked in by ppalmers, 13 years ago)

more reliability things

Line 
1 /*
2  * Copyright (C) 2005-2007 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 3 of the License, or
12  * (at your option) any later version.
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.01)
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 /*
38 #define ENTER_CRITICAL_SECTION { \
39     if (pthread_mutex_trylock(&m_compute_vars_lock) == EBUSY) { \
40         debugWarning(" (%p) lock clash\n", this); \
41         ENTER_CRITICAL_SECTION; \
42     } \
43     }
44 */
45 #define ENTER_CRITICAL_SECTION { \
46     pthread_mutex_lock(&m_compute_vars_lock); \
47     }
48 #define EXIT_CRITICAL_SECTION { \
49     pthread_mutex_unlock(&m_compute_vars_lock); \
50     }
51
52 IMPL_DEBUG_MODULE( CycleTimerHelper, CycleTimerHelper, DEBUG_LEVEL_NORMAL );
53
54 CycleTimerHelper::CycleTimerHelper(Ieee1394Service &parent, unsigned int update_period_us)
55     : m_Parent ( parent )
56     , m_ticks_per_update ( ((uint64_t)TICKS_PER_SECOND) * ((uint64_t)update_period_us) / 1000000ULL )
57     , m_usecs_per_update ( update_period_us )
58     , m_avg_wakeup_delay ( 0.0 )
59     , m_dll_e2 ( 0.0 )
60     , m_current_time_usecs ( 0 )
61     , m_next_time_usecs ( 0 )
62     , m_current_time_ticks ( 0 )
63     , m_next_time_ticks ( 0 )
64     , m_first_run ( true )
65     , m_Thread ( NULL )
66     , m_realtime ( false )
67     , m_priority ( 0 )
68 {
69     debugOutput( DEBUG_LEVEL_VERBOSE, "Create %p...\n", this);
70 }
71
72 CycleTimerHelper::CycleTimerHelper(Ieee1394Service &parent, unsigned int update_period_us, bool rt, int prio)
73     : m_Parent ( parent )
74     , m_ticks_per_update ( ((uint64_t)TICKS_PER_SECOND) * ((uint64_t)update_period_us) / 1000000ULL )
75     , m_usecs_per_update ( update_period_us )
76     , m_avg_wakeup_delay ( 0.0 )
77     , m_dll_e2 ( 0.0 )
78     , m_current_time_usecs ( 0 )
79     , m_next_time_usecs ( 0 )
80     , m_current_time_ticks ( 0 )
81     , m_next_time_ticks ( 0 )
82     , m_first_run ( true )
83     , m_Thread ( NULL )
84     , m_realtime ( rt )
85     , m_priority ( prio )
86 {
87     debugOutput( DEBUG_LEVEL_VERBOSE, "Create %p...\n", this);
88 }
89
90 CycleTimerHelper::~CycleTimerHelper()
91 {
92     if (m_Thread) {
93         m_Thread->Stop();
94         delete m_Thread;
95     }
96 }
97
98 bool
99 CycleTimerHelper::Start()
100 {
101     debugOutput( DEBUG_LEVEL_VERBOSE, "Start %p...\n", this);
102 #if IEEE1394SERVICE_USE_CYCLETIMER_DLL
103     m_Thread = new Util::PosixThread(this, m_realtime, m_priority,
104                                      PTHREAD_CANCEL_DEFERRED);
105     if(!m_Thread) {
106         debugFatal("No thread\n");
107         return false;
108     }
109     if (m_Thread->Start() != 0) {
110         debugFatal("Could not start update thread\n");
111         return false;
112     }
113 #endif
114     return true;
115 }
116
117 bool
118 CycleTimerHelper::Init()
119 {
120     debugOutput( DEBUG_LEVEL_VERBOSE, "Initialize %p...\n", this);
121     pthread_mutex_init(&m_compute_vars_lock, NULL);
122     return true;
123 }
124
125 bool
126 CycleTimerHelper::setThreadParameters(bool rt, int priority) {
127     debugOutput( DEBUG_LEVEL_VERBOSE, "(%p) switch to: (rt=%d, prio=%d)...\n", this, rt, priority);
128     if (priority > THREAD_MAX_RTPRIO) priority = THREAD_MAX_RTPRIO; // cap the priority
129     m_realtime = rt;
130     m_priority = priority;
131
132 #if IEEE1394SERVICE_USE_CYCLETIMER_DLL
133     if (m_Thread) {
134         if (m_realtime) {
135             m_Thread->AcquireRealTime(m_priority);
136         } else {
137             m_Thread->DropRealTime();
138         }
139     }
140 #endif
141
142     return true;
143 }
144
145 float
146 CycleTimerHelper::getRate()
147 {
148     float rate = (float)(diffTicks((uint64_t)m_next_time_ticks, (uint64_t)m_current_time_ticks));
149     rate /= (float)(m_next_time_usecs - m_current_time_usecs);
150     return rate;
151 }
152
153 float
154 CycleTimerHelper::getNominalRate()
155 {
156     float rate = ((double)TICKS_PER_SECOND) / 1000000.0;
157     return rate;
158 }
159
160 #if IEEE1394SERVICE_USE_CYCLETIMER_DLL
161
162 bool
163 CycleTimerHelper::Execute()
164 {
165     debugOutput( DEBUG_LEVEL_VERY_VERBOSE, "Execute %p...\n", this);
166     uint32_t cycle_timer;
167     uint64_t local_time;
168     if(!m_Parent.readCycleTimerReg(&cycle_timer, &local_time)) {
169         debugError("Could not read cycle timer register\n");
170         return false;
171     }
172     debugOutput( DEBUG_LEVEL_VERY_VERBOSE, " read : CTR: %11lu, local: %17llu\n",
173                     cycle_timer, local_time);
174
175     double usecs_late;
176     if (m_first_run) {
177         usecs_late = 0.0;
178         m_dll_e2 = m_ticks_per_update;
179         m_current_time_usecs = local_time;
180         m_next_time_usecs = m_current_time_usecs + m_usecs_per_update;
181         m_current_time_ticks = CYCLE_TIMER_TO_TICKS( cycle_timer );
182         m_next_time_ticks = addTicks( (uint64_t)m_current_time_ticks, (uint64_t)m_dll_e2);
183         debugOutput( DEBUG_LEVEL_VERBOSE, " First run\n");
184         debugOutput( DEBUG_LEVEL_VERBOSE, "  usecs/update: %lu, ticks/update: %lu, m_dll_e2: %f\n",
185                                           m_usecs_per_update, m_ticks_per_update, m_dll_e2);
186         debugOutput( DEBUG_LEVEL_VERBOSE, "  usecs current: %f, next: %f\n", m_current_time_usecs, m_next_time_usecs);
187         debugOutput( DEBUG_LEVEL_VERBOSE, "  ticks current: %f, next: %f\n", m_current_time_ticks, m_next_time_ticks);
188         m_first_run = false;
189     } else {
190
191         double diff = m_next_time_usecs - m_current_time_usecs;
192         debugOutput( DEBUG_LEVEL_VERY_VERBOSE, " usecs: local: %11llu current: %f next: %f, diff: %f\n",
193                     local_time, m_current_time_usecs, m_next_time_usecs, diff);
194
195         uint64_t cycle_timer_ticks = CYCLE_TIMER_TO_TICKS(cycle_timer);
196         usecs_late = ((double)local_time) - (m_next_time_usecs);
197
198         // we update the x-axis values
199         m_current_time_usecs = m_next_time_usecs;
200         m_next_time_usecs = (local_time - usecs_late) + m_usecs_per_update;
201         debugOutput( DEBUG_LEVEL_VERY_VERBOSE, " usecs: current: %f next: %f usecs_late=%f\n",
202                     m_current_time_usecs, m_next_time_usecs, usecs_late);
203
204         // and the y-axis values
205         double diff_ticks = diffTicks(cycle_timer_ticks, (int64_t)m_next_time_ticks);
206         m_current_time_ticks = m_next_time_ticks;
207         m_next_time_ticks = addTicks((uint64_t)m_current_time_ticks,
208                                      (uint64_t)((DLL_COEFF_B * diff_ticks) + m_dll_e2));
209         m_dll_e2 += DLL_COEFF_C * diff_ticks;
210         debugOutput( DEBUG_LEVEL_VERY_VERBOSE, " ticks: current: %f next: %f diff=%f\n",
211                     m_current_time_ticks, m_next_time_ticks, diff_ticks);
212
213         debugOutput( DEBUG_LEVEL_VERY_VERBOSE, " state: local: %11llu, dll_e2: %f, rate: %f\n",
214                     local_time, m_dll_e2, getRate());
215     }
216
217     // track the average wakeup delay
218     m_avg_wakeup_delay += 0.01 * usecs_late;
219
220     // FIXME: priority inversion!
221     ENTER_CRITICAL_SECTION;
222     m_current_vars.ticks = m_current_time_ticks;
223     m_current_vars.usecs = m_current_time_usecs;
224     m_current_vars.rate = getRate();
225     EXIT_CRITICAL_SECTION;
226
227     // wait for the next update period
228     int64_t time_to_sleep = (int64_t)m_next_time_usecs - m_Parent.getCurrentTimeAsUsecs();
229     time_to_sleep -= (int64_t)m_avg_wakeup_delay;
230     //int64_t time_to_sleep = m_usecs_per_update;
231     if (time_to_sleep > 0) {
232         debugOutput( DEBUG_LEVEL_VERY_VERBOSE, " sleeping %lld usecs (avg delay: %f)\n", time_to_sleep, m_avg_wakeup_delay);
233         usleep(time_to_sleep);
234     }
235     return true;
236 }
237
238 uint32_t
239 CycleTimerHelper::getCycleTimerTicks()
240 {
241     uint64_t now = m_Parent.getCurrentTimeAsUsecs();
242     return getCycleTimerTicks(now);
243 }
244
245 uint32_t
246 CycleTimerHelper::getCycleTimerTicks(uint64_t now)
247 {
248     uint32_t retval;
249     struct compute_vars my_vars;
250
251     // reduce lock contention
252     ENTER_CRITICAL_SECTION;
253     my_vars = m_current_vars;
254     EXIT_CRITICAL_SECTION;
255
256     double time_diff = now - my_vars.usecs;
257     double y_step_in_ticks = time_diff * my_vars.rate;
258     int64_t y_step_in_ticks_int = (int64_t)y_step_in_ticks;
259     uint64_t offset_in_ticks_int = (uint64_t)my_vars.ticks;
260
261     if (y_step_in_ticks_int > 0) {
262         retval = addTicks(offset_in_ticks_int, y_step_in_ticks_int);
263         debugOutput(DEBUG_LEVEL_VERY_VERBOSE, "y_step_in_ticks_int > 0: %lld, time_diff: %f, rate: %f, retval: %lu\n",
264                      y_step_in_ticks_int, time_diff, my_vars.rate, retval);
265     } else {
266         retval = substractTicks(offset_in_ticks_int, -y_step_in_ticks_int);
267
268         // this can happen if the update thread was woken up earlier than it should have been
269         debugOutput(DEBUG_LEVEL_VERY_VERBOSE, "y_step_in_ticks_int <= 0: %lld, time_diff: %f, rate: %f, retval: %lu\n",
270                      y_step_in_ticks_int, time_diff, my_vars.rate, retval);
271     }
272
273     return retval;
274 }
275
276 uint32_t
277 CycleTimerHelper::getCycleTimer()
278 {
279     return TICKS_TO_CYCLE_TIMER(getCycleTimerTicks());
280 }
281
282 uint32_t
283 CycleTimerHelper::getCycleTimer(uint64_t now)
284 {
285     return TICKS_TO_CYCLE_TIMER(getCycleTimerTicks(now));
286 }
287
288 #else
289
290 bool
291 CycleTimerHelper::Execute()
292 {
293     usleep(1000*1000);
294     return true;
295 }
296 uint32_t
297 CycleTimerHelper::getCycleTimerTicks()
298 {
299     uint32_t cycle_timer;
300     uint64_t local_time;
301     if(!m_Parent.readCycleTimerReg(&cycle_timer, &local_time)) {
302         debugError("Could not read cycle timer register\n");
303         return 0;
304     }
305     return CYCLE_TIMER_TO_TICKS(cycle_timer);
306 }
307
308 uint32_t
309 CycleTimerHelper::getCycleTimerTicks(uint64_t now)
310 {
311     return getCycleTimerTicks();
312 }
313
314 uint32_t
315 CycleTimerHelper::getCycleTimer()
316 {
317     uint32_t cycle_timer;
318     uint64_t local_time;
319     if(!m_Parent.readCycleTimerReg(&cycle_timer, &local_time)) {
320         debugError("Could not read cycle timer register\n");
321         return 0;
322     }
323     return cycle_timer;
324 }
325
326 uint32_t
327 CycleTimerHelper::getCycleTimer(uint64_t now)
328 {
329     return getCycleTimer();
330 }
331
332 #endif
333
334 void
335 CycleTimerHelper::setVerboseLevel(int l)
336 {
337     setDebugLevel(l);
338 }
Note: See TracBrowser for help on using the browser.