root/trunk/libffado/src/libutil/TimestampedBuffer.cpp

Revision 512, 31.6 kB (checked in by jwoithe, 15 years ago)

MOTU: more tweaks to improve reliability. Things are looking pretty good now.
MOTU: Commenced cleanup of MOTU code, removing temporary debug output etc.

Line 
1 /* $Id$ */
2
3 /*
4  *   FFADO Streaming API
5  *   FFADO = Firewire (pro-)audio for linux
6  *
7  *   http://ffado.sf.net
8  *
9  *   Copyright (C) 2005,2006,2007 Pieter Palmers <pieterpalmers@users.sourceforge.net>
10  *
11  *   This program is free software {} you can redistribute it and/or modify
12  *   it under the terms of the GNU General Public License as published by
13  *   the Free Software Foundation {} either version 2 of the License, or
14  *   (at your option) any later version.
15  *
16  *   This program is distributed in the hope that it will be useful,
17  *   but WITHOUT ANY WARRANTY {} without even the implied warranty of
18  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
19  *   GNU General Public License for more details.
20  *
21  *   You should have received a copy of the GNU General Public License
22  *   along with this program {} if not, write to the Free Software
23  *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
24  *
25  *
26  *
27  */
28
29 #include "libutil/Atomic.h"
30 #include "libstreaming/cycletimer.h"
31
32 #include "TimestampedBuffer.h"
33 #include "assert.h"
34
35 // FIXME: note that it will probably be better to use a DLL bandwidth that is
36 //        dependant on the sample rate
37
38
39 // #define DLL_BANDWIDTH (4800/48000.0)
40 #define DLL_BANDWIDTH (0.01)
41 #define DLL_PI        (3.141592653589793238)
42 #define DLL_SQRT2     (1.414213562373095049)
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 namespace Util {
48
49 IMPL_DEBUG_MODULE( TimestampedBuffer, TimestampedBuffer, DEBUG_LEVEL_VERBOSE );
50
51 TimestampedBuffer::TimestampedBuffer(TimestampedBufferClient *c)
52     : m_event_buffer(NULL), m_cluster_buffer(NULL),
53       m_event_size(0), m_events_per_frame(0), m_buffer_size(0),
54       m_bytes_per_frame(0), m_bytes_per_buffer(0),
55       m_wrap_at(0xFFFFFFFFFFFFFFFFLLU),
56       m_Client(c), m_framecounter(0),
57       m_tick_offset(0.0),
58       m_buffer_tail_timestamp(0.0),
59       m_buffer_next_tail_timestamp(0.0),
60       m_dll_e2(0.0), m_dll_b(DLL_COEFF_B), m_dll_c(DLL_COEFF_C),
61       m_nominal_rate(0.0), m_update_period(0)
62 {
63     pthread_mutex_init(&m_framecounter_lock, NULL);
64
65 }
66
67 TimestampedBuffer::~TimestampedBuffer() {
68     ffado_ringbuffer_free(m_event_buffer);
69     free(m_cluster_buffer);
70 }
71
72 /**
73  * \brief Set the nominal rate in frames/timeunit
74  *
75  * Sets the nominal rate in frames per time unit. This rate is used
76  * to initialize the DLL that will extract the effective rate based
77  * upon the timestamps it gets fed.
78  *
79  * @param r rate
80  * @return true if successful
81  */
82 bool TimestampedBuffer::setNominalRate(float r) {
83     m_nominal_rate=r;
84     debugOutput(DEBUG_LEVEL_VERBOSE," nominal rate=%e set to %e\n",
85                                     m_nominal_rate, r);
86     return true;
87 }
88
89 /**
90  * \brief Set the nominal update period (in frames)
91  *
92  * Sets the nominal update period. This period is the number of frames
93  * between two timestamp updates (hence buffer writes)
94  *
95  * @param n period in frames
96  * @return true if successful
97  */
98 bool TimestampedBuffer::setUpdatePeriod(unsigned int n) {
99     m_update_period=n;
100     return true;
101 }
102
103 /**
104  * \brief set the value at which timestamps should wrap around
105  * @param w value to wrap at
106  * @return true if successful
107  */
108 bool TimestampedBuffer::setWrapValue(ffado_timestamp_t w) {
109     m_wrap_at=w;
110     return true;
111 }
112 #include <math.h>
113
114 /**
115  * \brief return the effective rate
116  *
117  * Returns the effective rate calculated by the DLL.
118  *
119  * @return rate (in timeunits/frame)
120  */
121 float TimestampedBuffer::getRate() {
122     ffado_timestamp_t diff;
123    
124     pthread_mutex_lock(&m_framecounter_lock);
125     diff=m_buffer_next_tail_timestamp - m_buffer_tail_timestamp;
126     pthread_mutex_unlock(&m_framecounter_lock);
127    
128     debugOutput(DEBUG_LEVEL_VERY_VERBOSE,"getRate: %f/%f=%f\n",
129         (float)(diff),
130         (float)m_update_period,
131         ((float)(diff))/((float) m_update_period));
132    
133     // the maximal difference we can allow (64secs)
134     const ffado_timestamp_t max=m_wrap_at/((ffado_timestamp_t)2);
135
136     if(diff > max) {
137         diff -= m_wrap_at;
138     } else if (diff < -max) {
139         diff += m_wrap_at;
140     }
141    
142     float rate=((float)diff)/((float) m_update_period);
143     if (fabsf(m_nominal_rate - rate)>(m_nominal_rate*0.1)) {
144         debugWarning("(%p) rate (%10.5f) more that 10%% off nominal (rate=%10.5f, diff="TIMESTAMP_FORMAT_SPEC", update_period=%d)\n",
145                      this, rate,m_nominal_rate,diff, m_update_period);
146         dumpInfo();
147         return m_nominal_rate;
148     } else {
149         return rate;
150     }
151 }
152
153 /**
154  * \brief Sets the size of the events
155  * @param s event size in bytes
156  * @return true if successful
157  */
158 bool TimestampedBuffer::setEventSize(unsigned int s) {
159     m_event_size=s;
160
161     m_bytes_per_frame=m_event_size*m_events_per_frame;
162     m_bytes_per_buffer=m_bytes_per_frame*m_buffer_size;
163
164     return true;
165 }
166
167 /**
168  * \brief Sets the number of events per frame
169  * @param n number of events per frame
170  * @return true if successful
171  */
172 bool TimestampedBuffer::setEventsPerFrame(unsigned int n) {
173     m_events_per_frame=n;
174
175     m_bytes_per_frame=m_event_size*m_events_per_frame;
176     m_bytes_per_buffer=m_bytes_per_frame*m_buffer_size;
177
178     return true;
179 }
180 /**
181  * \brief Sets the buffer size in frames
182  * @param n number frames
183  * @return true if successful
184  */
185 bool TimestampedBuffer::setBufferSize(unsigned int n) {
186     m_buffer_size=n;
187
188     m_bytes_per_frame=m_event_size*m_events_per_frame;
189     m_bytes_per_buffer=m_bytes_per_frame*m_buffer_size;
190
191     return true;
192 }
193
194 /**
195  * Sets the buffer offset in ticks.
196  *
197  * A positive value means that the buffer is 'delayed' for nticks ticks.
198  *
199  * @note These offsets are only used when reading timestamps. Any function
200  *       that returns a timestamp will incorporate this offset.
201  * @param nframes the number of ticks (positive = delay buffer)
202  * @return true if successful
203  */
204 bool TimestampedBuffer::setTickOffset(ffado_timestamp_t nticks) {
205     debugOutput(DEBUG_LEVEL_VERBOSE,"Setting ticks offset to "TIMESTAMP_FORMAT_SPEC"\n",nticks);
206
207     // JMW: I think we need to update the internal DLL state to take account
208     // of the new offset.  Doing so certainly makes for a smoother MOTU
209     // startup.
210     pthread_mutex_lock(&m_framecounter_lock);
211     m_buffer_tail_timestamp = m_buffer_tail_timestamp - m_tick_offset + nticks;
212     m_buffer_next_tail_timestamp = (ffado_timestamp_t)((double)m_buffer_tail_timestamp + m_dll_e2);
213     m_tick_offset=nticks;
214     pthread_mutex_unlock(&m_framecounter_lock);
215
216     return true;
217 }
218
219 /**
220  * \brief Returns the current fill of the buffer
221  *
222  * This returns the buffer fill of the internal ringbuffer. This
223  * can only be used as an indication because it's state is not
224  * guaranteed to be consistent at all times due to threading issues.
225  *
226  * In order to get the number of frames in the buffer, use the
227  * getFrameCounter, getBufferHeadTimestamp, getBufferTailTimestamp
228  * functions
229  *
230  * @return the internal buffer fill in frames
231  */
232 unsigned int TimestampedBuffer::getBufferFill() {
233     return ffado_ringbuffer_read_space(m_event_buffer)/(m_bytes_per_frame);
234 }
235
236 /**
237  * \brief Initializes the TimestampedBuffer
238  *
239  * Initializes the TimestampedBuffer, should be called before anything else
240  * is done.
241  *
242  * @return true if successful
243  */
244 bool TimestampedBuffer::init() {
245     return true;
246 }
247
248 /**
249  * \brief Resets the TimestampedBuffer
250  *
251  * Resets the TimestampedBuffer, clearing the buffers and counters.
252  * (not true yet: Also resets the DLL to the nominal values.)
253  *
254  * \note when this is called, you should make sure that the buffer
255  *       tail timestamp gets set before continuing
256  *
257  * @return true if successful
258  */
259 bool TimestampedBuffer::reset() {
260     ffado_ringbuffer_reset(m_event_buffer);
261
262     resetFrameCounter();
263
264     return true;
265 }
266
267 /**
268  * \brief Prepares the TimestampedBuffer
269  *
270  * Prepare the TimestampedBuffer. This allocates all internal buffers and
271  * initializes all data structures.
272  *
273  * This should be called after parameters such as buffer size, event size etc.. are set,
274  * and before any read/write operations are performed.
275  *
276  * @return true if successful
277  */
278 bool TimestampedBuffer::prepare() {
279     debugOutput(DEBUG_LEVEL_VERBOSE,"Preparing buffer (%p)\n",this);
280     debugOutput(DEBUG_LEVEL_VERBOSE," Size=%u events, events/frame=%u, event size=%ubytes\n",
281                                         m_buffer_size,m_events_per_frame,m_event_size);
282
283     debugOutput(DEBUG_LEVEL_VERBOSE," update period %u\n",
284                                     m_update_period);
285     debugOutput(DEBUG_LEVEL_VERBOSE," nominal rate=%f\n",
286                                     m_nominal_rate);
287
288     debugOutput(DEBUG_LEVEL_VERBOSE," wrapping at "TIMESTAMP_FORMAT_SPEC"\n",m_wrap_at);
289
290     assert(m_buffer_size);
291     assert(m_events_per_frame);
292     assert(m_event_size);
293
294     assert(m_nominal_rate != 0.0L);
295     assert(m_update_period != 0);
296
297     if( !(m_event_buffer=ffado_ringbuffer_create(
298             (m_events_per_frame * m_buffer_size) * m_event_size))) {
299         debugFatal("Could not allocate memory event ringbuffer\n");
300         return false;
301     }
302
303     // allocate the temporary cluster buffer
304     if( !(m_cluster_buffer=(char *)calloc(m_events_per_frame,m_event_size))) {
305             debugFatal("Could not allocate temporary cluster buffer\n");
306         ffado_ringbuffer_free(m_event_buffer);
307         return false;
308     }
309
310     // init the DLL
311     m_dll_e2=m_nominal_rate * (float)m_update_period;
312
313     m_dll_b=((float)(DLL_COEFF_B));
314     m_dll_c=((float)(DLL_COEFF_C));
315    
316     // this will init the internal timestamps to a sensible value
317     setBufferTailTimestamp(m_buffer_tail_timestamp);
318    
319     return true;
320 }
321
322 /**
323  * @brief Insert a dummy frame to the head buffer
324  *
325  * Writes one frame of dummy data to the head of the buffer.
326  * This is to assist the phase sync of several buffers.
327  *
328  * Note: currently the dummy data is added to the tail of the
329  *       buffer, but without updating the timestamp.
330  *
331  * @return true if successful
332  */
333 bool TimestampedBuffer::writeDummyFrame() {
334
335     unsigned int write_size=m_event_size*m_events_per_frame;
336    
337     char dummy[write_size]; // one frame of garbage
338     memset(dummy,0,write_size);
339
340     // add the data payload to the ringbuffer
341     if (ffado_ringbuffer_write(m_event_buffer,dummy,write_size) < write_size)
342     {
343 //         debugWarning("writeFrames buffer overrun\n");
344         return false;
345     }
346
347 //     incrementFrameCounter(nframes,ts);
348    
349     // increment without updating the DLL
350     pthread_mutex_lock(&m_framecounter_lock);
351     m_framecounter++;
352     pthread_mutex_unlock(&m_framecounter_lock);
353    
354     return true;
355 }
356
357 /**
358  * @brief Write frames to the buffer
359  *
360  * Copies \ref nframes of frames from the buffer pointed to by \ref data to the
361  * internal ringbuffer. The time of the last frame in the buffer is set to \ref ts.
362  *
363  * @param nframes number of frames to copy
364  * @param data pointer to the frame buffer
365  * @param ts timestamp of the last frame copied
366  * @return true if successful
367  */
368 bool TimestampedBuffer::writeFrames(unsigned int nframes, char *data, ffado_timestamp_t ts) {
369
370     unsigned int write_size=nframes*m_event_size*m_events_per_frame;
371
372     // add the data payload to the ringbuffer
373     if (ffado_ringbuffer_write(m_event_buffer,data,write_size) < write_size)
374     {
375 //         debugWarning("writeFrames buffer overrun\n");
376         return false;
377     }
378
379     incrementFrameCounter(nframes,ts);
380
381     return true;
382
383 }
384 /**
385  * @brief Read frames from the buffer
386  *
387  * Copies \ref nframes of frames from the internal buffer to the data buffer pointed
388  * to by \ref data.
389  *
390  * @param nframes number of frames to copy
391  * @param data pointer to the frame buffer
392  * @return true if successful
393  */
394 bool TimestampedBuffer::readFrames(unsigned int nframes, char *data) {
395
396     unsigned int read_size=nframes*m_event_size*m_events_per_frame;
397
398     // get the data payload to the ringbuffer
399     if ((ffado_ringbuffer_read(m_event_buffer,data,read_size)) < read_size)
400     {
401 //         debugWarning("readFrames buffer underrun\n");
402         return false;
403     }
404
405     decrementFrameCounter(nframes);
406
407     return true;
408
409 }
410
411 /**
412  * @brief Performs block processing write of frames
413  *
414  * This function allows for zero-copy writing into the ringbuffer.
415  * It calls the client's processWriteBlock function to write frames
416  * into the internal buffer's data area, in a thread safe fashion.
417  *
418  * It also updates the timestamp.
419  *
420  * @param nbframes number of frames to process
421  * @param ts timestamp of the last frame written to the buffer
422  * @return true if successful
423  */
424 bool TimestampedBuffer::blockProcessWriteFrames(unsigned int nbframes, ffado_timestamp_t ts) {
425
426     debugOutput( DEBUG_LEVEL_VERY_VERBOSE, "Transferring period...\n");
427     int xrun;
428     unsigned int offset=0;
429
430     ffado_ringbuffer_data_t vec[2];
431     // we received one period of frames
432     // this is period_size*dimension of events
433     unsigned int events2write=nbframes*m_events_per_frame;
434     unsigned int bytes2write=events2write*m_event_size;
435
436     /* write events2write bytes to the ringbuffer
437     *  first see if it can be done in one read.
438     *  if so, ok.
439     *  otherwise write up to a multiple of clusters directly to the buffer
440     *  then do the buffer wrap around using ringbuffer_write
441     *  then write the remaining data directly to the buffer in a third pass
442     *  Make sure that we cannot end up on a non-cluster aligned position!
443     */
444     unsigned int cluster_size=m_events_per_frame*m_event_size;
445
446     while(bytes2write>0) {
447         int byteswritten=0;
448
449         unsigned int frameswritten=(nbframes*cluster_size-bytes2write)/cluster_size;
450         offset=frameswritten;
451
452         ffado_ringbuffer_get_write_vector(m_event_buffer, vec);
453
454         if(vec[0].len==0) { // this indicates a full event buffer
455             debugError("Event buffer overrun in buffer %p\n",this);
456             break;
457         }
458
459         /* if we don't take care we will get stuck in an infinite loop
460         * because we align to a cluster boundary later
461         * the remaining nb of bytes in one write operation can be
462         * smaller than one cluster
463         * this can happen because the ringbuffer size is always a power of 2
464         */
465         if(vec[0].len<cluster_size) {
466
467             // encode to the temporary buffer
468             xrun = m_Client->processWriteBlock(m_cluster_buffer, 1, offset);
469
470             if(xrun<0) {
471                 // xrun detected
472                 debugError("Frame buffer underrun in buffer %p\n",this);
473                 return false;
474             }
475
476             // use the ringbuffer function to write one cluster
477             // the write function handles the wrap around.
478             ffado_ringbuffer_write(m_event_buffer,
479                          m_cluster_buffer,
480                          cluster_size);
481
482             // we advanced one cluster_size
483             bytes2write-=cluster_size;
484
485         } else { //
486
487             if(bytes2write>vec[0].len) {
488                 // align to a cluster boundary
489                 byteswritten=vec[0].len-(vec[0].len%cluster_size);
490             } else {
491                 byteswritten=bytes2write;
492             }
493
494             xrun = m_Client->processWriteBlock(vec[0].buf,
495                          byteswritten/cluster_size,
496                          offset);
497
498             if(xrun<0) {
499                     // xrun detected
500                 debugError("Frame buffer underrun in buffer %p\n",this);
501                 return false; // FIXME: return false ?
502             }
503
504             ffado_ringbuffer_write_advance(m_event_buffer, byteswritten);
505             bytes2write -= byteswritten;
506         }
507
508         // the bytes2write should always be cluster aligned
509         assert(bytes2write%cluster_size==0);
510
511     }
512
513     incrementFrameCounter(nbframes,ts);
514
515     return true;
516
517 }
518
519 /**
520  * @brief Performs block processing read of frames
521  *
522  * This function allows for zero-copy reading from the ringbuffer.
523  * It calls the client's processReadBlock function to read frames
524  * directly from the internal buffer's data area, in a thread safe
525  * fashion.
526  *
527  * @param nbframes number of frames to process
528  * @return true if successful
529  */
530 bool TimestampedBuffer::blockProcessReadFrames(unsigned int nbframes) {
531
532     debugOutput( DEBUG_LEVEL_VERY_VERBOSE, "Reading %u from buffer (%p)...\n", nbframes, this);
533
534     int xrun;
535     unsigned int offset=0;
536
537     ffado_ringbuffer_data_t vec[2];
538     // we received one period of frames on each connection
539     // this is period_size*dimension of events
540
541     unsigned int events2read=nbframes*m_events_per_frame;
542     unsigned int bytes2read=events2read*m_event_size;
543     /* read events2read bytes from the ringbuffer
544     *  first see if it can be done in one read.
545     *  if so, ok.
546     *  otherwise read up to a multiple of clusters directly from the buffer
547     *  then do the buffer wrap around using ringbuffer_read
548     *  then read the remaining data directly from the buffer in a third pass
549     *  Make sure that we cannot end up on a non-cluster aligned position!
550     */
551     unsigned int cluster_size=m_events_per_frame*m_event_size;
552
553     while(bytes2read>0) {
554         unsigned int framesread=(nbframes*cluster_size-bytes2read)/cluster_size;
555         offset=framesread;
556
557         int bytesread=0;
558
559         ffado_ringbuffer_get_read_vector(m_event_buffer, vec);
560
561         if(vec[0].len==0) { // this indicates an empty event buffer
562             debugError("Event buffer underrun in buffer %p\n",this);
563             return false;
564         }
565
566         /* if we don't take care we will get stuck in an infinite loop
567         * because we align to a cluster boundary later
568         * the remaining nb of bytes in one read operation can be smaller than one cluster
569         * this can happen because the ringbuffer size is always a power of 2
570                 */
571         if(vec[0].len<cluster_size) {
572             // use the ringbuffer function to read one cluster
573             // the read function handles wrap around
574             ffado_ringbuffer_read(m_event_buffer,m_cluster_buffer,cluster_size);
575
576             assert(m_Client);
577             xrun = m_Client->processReadBlock(m_cluster_buffer, 1, offset);
578
579             if(xrun<0) {
580                 // xrun detected
581                 debugError("Frame buffer overrun in buffer %p\n",this);
582                     return false;
583             }
584
585             // we advanced one cluster_size
586             bytes2read-=cluster_size;
587
588         } else { //
589
590             if(bytes2read>vec[0].len) {
591                 // align to a cluster boundary
592                 bytesread=vec[0].len-(vec[0].len%cluster_size);
593             } else {
594                 bytesread=bytes2read;
595             }
596
597             assert(m_Client);
598             xrun = m_Client->processReadBlock(vec[0].buf, bytesread/cluster_size, offset);
599
600             if(xrun<0) {
601                 // xrun detected
602                 debugError("Frame buffer overrun in buffer %p\n",this);
603                 return false;
604             }
605
606             ffado_ringbuffer_read_advance(m_event_buffer, bytesread);
607             bytes2read -= bytesread;
608         }
609
610         // the bytes2read should always be cluster aligned
611         assert(bytes2read%cluster_size==0);
612     }
613
614     decrementFrameCounter(nbframes);
615
616     return true;
617 }
618
619 /**
620  * @brief Sets the buffer tail timestamp.
621  *
622  * Set the buffer tail timestamp to \ref new_timestamp. This will recalculate
623  * the internal state such that the buffer's timeframe starts at
624  * \ref new_timestamp.
625  *
626  * This is thread safe.
627  *
628  * @note considers offsets
629  *
630  * @param new_timestamp
631  */
632 void TimestampedBuffer::setBufferTailTimestamp(ffado_timestamp_t new_timestamp) {
633
634     // add the offsets
635     ffado_timestamp_t ts=new_timestamp;
636     ts += m_tick_offset;
637
638     if (ts >= m_wrap_at) {
639         ts -= m_wrap_at;
640     } else if (ts < 0) {
641         ts += m_wrap_at;
642     }
643
644 #ifdef DEBUG
645     if (new_timestamp >= m_wrap_at) {
646         debugWarning("timestamp not wrapped: "TIMESTAMP_FORMAT_SPEC"\n",new_timestamp);
647     }
648     if ((ts >= m_wrap_at) || (ts < 0 )) {
649         debugWarning("ts not wrapped correctly: "TIMESTAMP_FORMAT_SPEC"\n",ts);
650     }
651 #endif
652
653     pthread_mutex_lock(&m_framecounter_lock);
654
655     m_buffer_tail_timestamp = ts;
656
657     m_dll_e2=m_update_period * (double)m_nominal_rate;
658     m_buffer_next_tail_timestamp = (ffado_timestamp_t)((double)m_buffer_tail_timestamp + m_dll_e2);
659
660     pthread_mutex_unlock(&m_framecounter_lock);
661
662     debugOutput(DEBUG_LEVEL_VERY_VERBOSE, "Set buffer tail timestamp for (%p) to "
663                                           TIMESTAMP_FORMAT_SPEC" => "TIMESTAMP_FORMAT_SPEC", NTS="
664                                           TIMESTAMP_FORMAT_SPEC", DLL2=%f, RATE=%f\n",
665                 this, new_timestamp, ts, m_buffer_next_tail_timestamp, m_dll_e2, m_nominal_rate);
666
667 }
668
669 /**
670  * @brief Sets the buffer head timestamp.
671  *
672  * Set the buffer tail timestamp such that the buffer head timestamp becomes
673  * \ref new_timestamp. This does not consider offsets, because it's use is to
674  * make sure the following is true after setBufferHeadTimestamp(x):
675  *   x == getBufferHeadTimestamp()
676  *
677  * This is thread safe.
678  *
679  * @param new_timestamp
680  */
681 void TimestampedBuffer::setBufferHeadTimestamp(ffado_timestamp_t new_timestamp) {
682
683 #ifdef DEBUG
684     if (new_timestamp >= m_wrap_at) {
685         debugWarning("timestamp not wrapped: "TIMESTAMP_FORMAT_SPEC"\n",new_timestamp);
686     }
687 #endif
688
689     ffado_timestamp_t ts=new_timestamp;
690
691     pthread_mutex_lock(&m_framecounter_lock);
692
693     // add the time
694     ts += (ffado_timestamp_t)(m_nominal_rate * (float)m_framecounter);
695
696     if (ts >= m_wrap_at) {
697         ts -= m_wrap_at;
698     } else if (ts < 0) {
699         ts += m_wrap_at;
700     }
701
702     m_buffer_tail_timestamp = ts;
703
704     m_dll_e2=m_update_period * (double)m_nominal_rate;
705     m_buffer_next_tail_timestamp = (ffado_timestamp_t)((double)m_buffer_tail_timestamp + m_dll_e2);
706
707     pthread_mutex_unlock(&m_framecounter_lock);
708
709     debugOutput(DEBUG_LEVEL_VERY_VERBOSE, "Set buffer head timestamp for (%p) to "TIMESTAMP_FORMAT_SPEC" => "
710                                           TIMESTAMP_FORMAT_SPEC", NTS="TIMESTAMP_FORMAT_SPEC", DLL2=%f, RATE=%f\n",
711                 this, new_timestamp, ts, m_buffer_next_tail_timestamp, m_dll_e2, m_nominal_rate);
712
713 }
714
715 /**
716  * \brief return the timestamp of the first frame in the buffer
717  *
718  * This function returns the timestamp of the very first sample in
719  * the StreamProcessor's buffer. It also returns the framecounter value
720  * for which this timestamp is valid.
721  *
722  * @param ts address to store the timestamp in
723  * @param fc address to store the associated framecounter in
724  */
725 void TimestampedBuffer::getBufferHeadTimestamp(ffado_timestamp_t *ts, signed int *fc) {
726     // NOTE: this is still ok with threads, because we use *fc to compute
727     //       the timestamp
728     *fc = m_framecounter;
729     *ts=getTimestampFromTail(*fc);
730 }
731
732 /**
733  * \brief return the timestamp of the last frame in the buffer
734  *
735  * This function returns the timestamp of the last frame in
736  * the StreamProcessor's buffer. It also returns the framecounter
737  * value for which this timestamp is valid.
738  *
739  * @param ts address to store the timestamp in
740  * @param fc address to store the associated framecounter in
741  */
742 void TimestampedBuffer::getBufferTailTimestamp(ffado_timestamp_t *ts, signed int *fc) {
743     pthread_mutex_lock(&m_framecounter_lock);
744     *fc = m_framecounter;
745     *ts = m_buffer_tail_timestamp;
746     pthread_mutex_unlock(&m_framecounter_lock);
747 }
748
749 /**
750  * @brief Get timestamp for a specific position from the buffer tail
751  *
752  * Returns the timestamp for a position that is nframes earlier than the
753  * buffer tail
754  *
755  * @param nframes number of frames
756  * @return timestamp value
757  */
758 ffado_timestamp_t TimestampedBuffer::getTimestampFromTail(int nframes)
759 {
760     // ts(x) = m_buffer_tail_timestamp -
761     //         (m_buffer_next_tail_timestamp - m_buffer_tail_timestamp)/(samples_between_updates)*(x)
762     ffado_timestamp_t diff;
763     float rate;
764     ffado_timestamp_t timestamp;
765    
766     pthread_mutex_lock(&m_framecounter_lock);
767    
768     diff=m_buffer_next_tail_timestamp - m_buffer_tail_timestamp;
769     timestamp=m_buffer_tail_timestamp;
770    
771     pthread_mutex_unlock(&m_framecounter_lock);
772    
773     if (diff < 0) diff += m_wrap_at;
774     rate=(float)diff / (float)m_update_period;
775
776     timestamp-=(ffado_timestamp_t)((nframes) * rate);
777
778     if(timestamp >= m_wrap_at) {
779         timestamp -= m_wrap_at;
780     } else if(timestamp < 0) {
781         timestamp += m_wrap_at;
782     }
783
784     return timestamp;
785 }
786
787 /**
788  * @brief Get timestamp for a specific position from the buffer head
789  *
790  * Returns the timestamp for a position that is nframes later than the
791  * buffer head
792  *
793  * @param nframes number of frames
794  * @return timestamp value
795  */
796 ffado_timestamp_t TimestampedBuffer::getTimestampFromHead(int nframes)
797 {
798     return getTimestampFromTail(m_framecounter-nframes);
799 }
800
801 /**
802  * Resets the frame counter, in a atomic way. This
803  * is thread safe.
804  */
805 void TimestampedBuffer::resetFrameCounter() {
806     pthread_mutex_lock(&m_framecounter_lock);
807     m_framecounter = 0;
808     pthread_mutex_unlock(&m_framecounter_lock);
809 }
810
811 /**
812  * Decrements the frame counter in a thread safe way.
813  *
814  * @param nbframes number of frames to decrement
815  */
816 void TimestampedBuffer::decrementFrameCounter(int nbframes) {
817     pthread_mutex_lock(&m_framecounter_lock);
818     m_framecounter -= nbframes;
819     pthread_mutex_unlock(&m_framecounter_lock);
820 }
821
822 /**
823  * Increments the frame counter in a thread safe way.
824  * Also updates the timestamp.
825  *
826  * @note the offsets defined by setTicksOffset and setFrameOffset
827  *       are added here.
828  *
829  * @param nbframes the number of frames to add
830  * @param new_timestamp the new timestamp
831  */
832 void TimestampedBuffer::incrementFrameCounter(int nbframes, ffado_timestamp_t new_timestamp) {
833
834     // add the offsets
835     ffado_timestamp_t diff;
836    
837     pthread_mutex_lock(&m_framecounter_lock);
838     diff=m_buffer_next_tail_timestamp - m_buffer_tail_timestamp;
839     pthread_mutex_unlock(&m_framecounter_lock);
840
841     if (diff < 0) diff += m_wrap_at;
842
843 #ifdef DEBUG
844     float rate=(float)diff / (float)m_update_period;
845 #endif
846
847     ffado_timestamp_t ts=new_timestamp;
848     ts += m_tick_offset;
849
850     if (ts >= m_wrap_at) {
851         ts -= m_wrap_at;
852     } else if (ts < 0) {
853         ts += m_wrap_at;
854     }
855
856     debugOutput(DEBUG_LEVEL_VERY_VERBOSE, "Setting buffer tail timestamp for (%p) to "
857                                           TIMESTAMP_FORMAT_SPEC" => "TIMESTAMP_FORMAT_SPEC"\n",
858                 this, new_timestamp, ts);
859
860 #ifdef DEBUG
861     if (new_timestamp >= m_wrap_at) {
862         debugWarning("timestamp not wrapped: "TIMESTAMP_FORMAT_SPEC"\n",new_timestamp);
863     }
864     if ((ts >= m_wrap_at) || (ts < 0 )) {
865         debugWarning("ts not wrapped correctly: "TIMESTAMP_FORMAT_SPEC"\n",ts);
866     }
867 #endif
868 // FIXME: JMW: at some points during startup the timestamp doesn't change.
869 // This still needs to be verified in more detail. 
870 if (ts>m_buffer_tail_timestamp-1 && ts<m_buffer_tail_timestamp+1) {
871   pthread_mutex_lock(&m_framecounter_lock);
872   m_framecounter += nbframes;
873   pthread_mutex_unlock(&m_framecounter_lock);
874   return;
875 }
876    
877     // update the DLL
878     pthread_mutex_lock(&m_framecounter_lock);
879     diff = ts-m_buffer_next_tail_timestamp;
880     pthread_mutex_unlock(&m_framecounter_lock);
881
882 #ifdef DEBUG
883     if ((diff > ((ffado_timestamp_t)1000)) || (diff < ((ffado_timestamp_t)-1000))) {
884         debugWarning("(%p) difference rather large: "TIMESTAMP_FORMAT_SPEC", "TIMESTAMP_FORMAT_SPEC", "TIMESTAMP_FORMAT_SPEC"\n",
885             this, diff, ts, m_buffer_next_tail_timestamp);
886     }
887 #endif
888
889     // idea to implement it for nbframes values that differ from m_update_period:
890     // diff = diff * nbframes/m_update_period
891     // m_buffer_next_tail_timestamp = m_buffer_tail_timestamp + diff
892
893     debugOutput(DEBUG_LEVEL_VERY_VERBOSE, "(%p): diff="TIMESTAMP_FORMAT_SPEC" ",
894                 this, diff);
895
896     // the maximal difference we can allow (64secs)
897     const ffado_timestamp_t max=m_wrap_at/2;
898
899     if(diff > max) {
900         diff -= m_wrap_at;
901     } else if (diff < -max) {
902         diff += m_wrap_at;
903     }
904
905     float err=diff;
906
907     debugOutputShort(DEBUG_LEVEL_VERY_VERBOSE, "diff2="TIMESTAMP_FORMAT_SPEC" err=%f\n",
908                     diff, err);
909     debugOutput(DEBUG_LEVEL_VERY_VERBOSE, "B: FC=%10u, TS="TIMESTAMP_FORMAT_SPEC", NTS="TIMESTAMP_FORMAT_SPEC"\n",
910                     m_framecounter, m_buffer_tail_timestamp, m_buffer_next_tail_timestamp);
911
912     pthread_mutex_lock(&m_framecounter_lock);
913     m_framecounter += nbframes;
914
915     m_buffer_tail_timestamp=m_buffer_next_tail_timestamp;
916     m_buffer_next_tail_timestamp += (ffado_timestamp_t)(m_dll_b * err + m_dll_e2);
917 //    m_buffer_tail_timestamp=ts;
918 //    m_buffer_next_tail_timestamp += (ffado_timestamp_t)(m_dll_b * err + m_dll_e2);
919    
920     m_dll_e2 += m_dll_c*err;
921    
922 //     debugOutputShort(DEBUG_LEVEL_VERBOSE, "%p %llu %lld %llu %llu %e %e\n", this, new_timestamp, diff, m_buffer_tail_timestamp, m_buffer_next_tail_timestamp, m_dll_e2, 24576000.0/getRate());
923
924     debugOutput(DEBUG_LEVEL_VERY_VERBOSE, "U: FC=%10u, TS="TIMESTAMP_FORMAT_SPEC", NTS="TIMESTAMP_FORMAT_SPEC"\n",
925                     m_framecounter, m_buffer_tail_timestamp, m_buffer_next_tail_timestamp);
926
927     if (m_buffer_next_tail_timestamp >= m_wrap_at) {
928         debugOutput(DEBUG_LEVEL_VERY_VERBOSE, "Unwrapping next tail timestamp: "TIMESTAMP_FORMAT_SPEC"",
929                 m_buffer_next_tail_timestamp);
930
931         m_buffer_next_tail_timestamp -= m_wrap_at;
932
933         debugOutputShort(DEBUG_LEVEL_VERY_VERBOSE, " => "TIMESTAMP_FORMAT_SPEC"\n",
934                 m_buffer_next_tail_timestamp);
935
936     }
937
938     debugOutput(DEBUG_LEVEL_VERY_VERBOSE, "A: TS="TIMESTAMP_FORMAT_SPEC", NTS="TIMESTAMP_FORMAT_SPEC", DLLe2=%f, RATE=%f\n",
939                 m_buffer_tail_timestamp, m_buffer_next_tail_timestamp, m_dll_e2, rate);
940
941     pthread_mutex_unlock(&m_framecounter_lock);
942
943     if(m_buffer_tail_timestamp>=m_wrap_at) {
944         debugError("Wrapping failed for m_buffer_tail_timestamp! "TIMESTAMP_FORMAT_SPEC"\n",m_buffer_tail_timestamp);
945         debugOutput(DEBUG_LEVEL_VERY_VERBOSE, " IN="TIMESTAMP_FORMAT_SPEC", TS="TIMESTAMP_FORMAT_SPEC", NTS="TIMESTAMP_FORMAT_SPEC"\n",
946                     ts, m_buffer_tail_timestamp, m_buffer_next_tail_timestamp);
947
948     }
949     if(m_buffer_next_tail_timestamp>=m_wrap_at) {
950         debugError("Wrapping failed for m_buffer_next_tail_timestamp! "TIMESTAMP_FORMAT_SPEC"\n",m_buffer_next_tail_timestamp);
951         debugOutput(DEBUG_LEVEL_VERY_VERBOSE, " IN="TIMESTAMP_FORMAT_SPEC", TS="TIMESTAMP_FORMAT_SPEC", NTS="TIMESTAMP_FORMAT_SPEC"\n",
952                     ts, m_buffer_tail_timestamp, m_buffer_next_tail_timestamp);
953     }
954    
955     if(m_buffer_tail_timestamp==m_buffer_next_tail_timestamp) {
956         debugError("Current and next timestamps are equal: "TIMESTAMP_FORMAT_SPEC" "TIMESTAMP_FORMAT_SPEC"\n",
957                    m_buffer_tail_timestamp,m_buffer_next_tail_timestamp);
958    
959     }
960
961     // this DLL allows the calculation of any sample timestamp relative to the buffer tail,
962     // to the next period and beyond (through extrapolation)
963     //
964     // ts(x) = m_buffer_tail_timestamp +
965     //         (m_buffer_next_tail_timestamp - m_buffer_tail_timestamp)/(samples_between_updates)*x
966
967 }
968
969 /**
970  * @brief Print status info.
971  */
972 void TimestampedBuffer::dumpInfo() {
973
974     ffado_timestamp_t ts_head;
975     signed int fc;
976     getBufferHeadTimestamp(&ts_head,&fc);
977
978 #ifdef DEBUG
979     ffado_timestamp_t diff=(ffado_timestamp_t)ts_head - (ffado_timestamp_t)m_buffer_tail_timestamp;
980 #endif
981
982     debugOutputShort( DEBUG_LEVEL_NORMAL, "  TimestampedBuffer (%p) info:\n",this);
983     debugOutputShort( DEBUG_LEVEL_NORMAL, "  Frame counter         : %d\n", m_framecounter);
984     debugOutputShort( DEBUG_LEVEL_NORMAL, "  Buffer head timestamp : "TIMESTAMP_FORMAT_SPEC"\n",ts_head);
985     debugOutputShort( DEBUG_LEVEL_NORMAL, "  Buffer tail timestamp : "TIMESTAMP_FORMAT_SPEC"\n",m_buffer_tail_timestamp);
986     debugOutputShort( DEBUG_LEVEL_NORMAL, "  Next tail timestamp   : "TIMESTAMP_FORMAT_SPEC"\n",m_buffer_next_tail_timestamp);
987     debugOutputShort( DEBUG_LEVEL_NORMAL, "  Head - Tail           : "TIMESTAMP_FORMAT_SPEC"\n",diff);
988     debugOutputShort( DEBUG_LEVEL_NORMAL, "  rate                  : %f (%f)\n",m_dll_e2,m_dll_e2/m_update_period);
989 }
990
991 } // end of namespace Util
Note: See TracBrowser for help on using the browser.