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

Revision 2802, 37.7 kB (checked in by jwoithe, 3 years ago)

Cosmetic: "Firewire" becomes "FireWire?".

Officially both the "F" and "W" were capitalised in the FireWire? name, so
reflect this throughout FFADO's source tree. This mostly affects comments.

This patch originated from pander on the ffado-devel mailing list. To
maintain consistency, the committed version has been expanded to include
files not originally included in the original patch.

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