root/branches/ppalmers-streaming/src/libutil/TimestampedBuffer.cpp

Revision 707, 33.8 kB (checked in by ppalmers, 15 years ago)

- code cleanup
- make transmit handler AMDTP compliant (don't send too much in advance)

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