fixed-point version of the high-pass seems to work now.
[speexdsp.git] / libspeex / jitter.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: speex_jitter.h
3
4    Adaptive jitter buffer for Speex
5
6    Redistribution and use in source and binary forms, with or without
7    modification, are permitted provided that the following conditions
8    are met:
9    
10    - Redistributions of source code must retain the above copyright
11    notice, this list of conditions and the following disclaimer.
12    
13    - Redistributions in binary form must reproduce the above copyright
14    notice, this list of conditions and the following disclaimer in the
15    documentation and/or other materials provided with the distribution.
16    
17    - Neither the name of the Xiph.org Foundation nor the names of its
18    contributors may be used to endorse or promote products derived from
19    this software without specific prior written permission.
20    
21    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
24    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
25    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
26    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
27    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
28    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
29    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
30    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
31    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32
33 */
34
35 #ifdef HAVE_CONFIG_H
36 #include "config.h"
37 #endif
38
39
40 #include "misc.h"
41 #include <speex/speex.h>
42 #include <speex/speex_bits.h>
43 #include <speex/speex_jitter.h>
44 #include <stdio.h>
45
46 #define LATE_BINS 10
47 #define MAX_MARGIN 30                     /**< Number of bins in margin histogram */
48
49 #define SPEEX_JITTER_MAX_BUFFER_SIZE 200   /**< Maximum number of packets in jitter buffer */
50
51
52
53 #define GT32(a,b) (((spx_int32_t)((a)-(b)))>0)
54 #define GE32(a,b) (((spx_int32_t)((a)-(b)))>=0)
55 #define LT32(a,b) (((spx_int32_t)((a)-(b)))<0)
56 #define LE32(a,b) (((spx_int32_t)((a)-(b)))<=0)
57
58 /** Jitter buffer structure */
59 struct JitterBuffer_ {
60    spx_uint32_t pointer_timestamp;                                        /**< Timestamp of what we will *get* next */
61    spx_uint32_t current_timestamp;                                        /**< Timestamp of the local clock (what we will *play* next) */
62
63    char *buf[SPEEX_JITTER_MAX_BUFFER_SIZE];                               /**< Buffer of packets (NULL if slot is free) */
64    spx_uint32_t timestamp[SPEEX_JITTER_MAX_BUFFER_SIZE];                  /**< Timestamp of packet                 */
65    int span[SPEEX_JITTER_MAX_BUFFER_SIZE];                                /**< Timestamp of packet                 */
66    int len[SPEEX_JITTER_MAX_BUFFER_SIZE];                                 /**< Number of bytes in packet           */
67
68    int tick_size;                                                         /**< Output granularity                  */
69    int reset_state;                                                       /**< True if state was just reset        */
70    int buffer_margin;                                                     /**< How many frames we want to keep in the buffer (lower bound) */
71    
72    int lost_count;                                                        /**< Number of consecutive lost packets  */
73    float shortterm_margin[MAX_MARGIN];                                    /**< Short term margin histogram         */
74    float longterm_margin[MAX_MARGIN];                                     /**< Long term margin histogram          */
75    float loss_rate;                                                       /**< Average loss rate                   */
76 };
77
78 /** Initialise jitter buffer */
79 JitterBuffer *jitter_buffer_init(int tick)
80 {
81    JitterBuffer *jitter = speex_alloc(sizeof(JitterBuffer));
82    if (jitter)
83    {
84       int i;
85       for (i=0;i<SPEEX_JITTER_MAX_BUFFER_SIZE;i++)
86          jitter->buf[i]=NULL;
87       jitter->tick_size = tick;
88       jitter->buffer_margin = 1;
89       jitter_buffer_reset(jitter);
90    }
91    return jitter;
92 }
93
94 /** Reset jitter buffer */
95 void jitter_buffer_reset(JitterBuffer *jitter)
96 {
97    int i;
98    for (i=0;i<SPEEX_JITTER_MAX_BUFFER_SIZE;i++)
99    {
100       if (jitter->buf[i])
101       {
102          speex_free(jitter->buf[i]);
103          jitter->buf[i] = NULL;
104       }
105    }
106    /* Timestamp is actually undefined at this point */
107    jitter->pointer_timestamp = 0;
108    jitter->current_timestamp = 0;
109    jitter->reset_state = 1;
110    jitter->lost_count = 0;
111    jitter->loss_rate = 0;
112    for (i=0;i<MAX_MARGIN;i++)
113    {
114       jitter->shortterm_margin[i] = 0;
115       jitter->longterm_margin[i] = 0;
116    }
117    /*fprintf (stderr, "reset\n");*/
118 }
119
120 /** Destroy jitter buffer */
121 void jitter_buffer_destroy(JitterBuffer *jitter)
122 {
123    jitter_buffer_reset(jitter);
124    speex_free(jitter);
125 }
126
127 /** Put one packet into the jitter buffer */
128 void jitter_buffer_put(JitterBuffer *jitter, const JitterBufferPacket *packet)
129 {
130    int i,j;
131    spx_int32_t arrival_margin;
132    /*fprintf (stderr, "put packet %d %d\n", timestamp, span);*/
133    if (jitter->reset_state)
134    {
135       jitter->reset_state=0;
136       jitter->pointer_timestamp = packet->timestamp;
137       jitter->current_timestamp = packet->timestamp;
138       /*fprintf(stderr, "reset to %d\n", timestamp);*/
139    }
140    
141    /* Cleanup buffer (remove old packets that weren't played) */
142    for (i=0;i<SPEEX_JITTER_MAX_BUFFER_SIZE;i++)
143    {
144       if (jitter->buf[i] && LE32(jitter->timestamp[i] + jitter->span[i], jitter->pointer_timestamp))
145       {
146          /*fprintf (stderr, "cleaned (not played)\n");*/
147          speex_free(jitter->buf[i]);
148          jitter->buf[i] = NULL;
149       }
150    }
151
152    /*Find an empty slot in the buffer*/
153    for (i=0;i<SPEEX_JITTER_MAX_BUFFER_SIZE;i++)
154    {
155       if (jitter->buf[i]==NULL)
156          break;
157    }
158
159    /*fprintf(stderr, "%d %d %f\n", timestamp, jitter->pointer_timestamp, jitter->drift_average);*/
160    /*No place left in the buffer*/
161    if (i==SPEEX_JITTER_MAX_BUFFER_SIZE)
162    {
163       int earliest=jitter->timestamp[0];
164       i=0;
165       for (j=1;j<SPEEX_JITTER_MAX_BUFFER_SIZE;j++)
166       {
167          if (!jitter->buf[i] || LT32(jitter->timestamp[j],earliest))
168          {
169             earliest = jitter->timestamp[j];
170             i=j;
171          }
172       }
173       speex_free(jitter->buf[i]);
174       jitter->buf[i]=NULL;
175       if (jitter->lost_count>20)
176       {
177          jitter_buffer_reset(jitter);
178       }
179       /*fprintf (stderr, "Buffer is full, discarding earliest frame %d (currently at %d)\n", timestamp, jitter->pointer_timestamp);*/      
180    }
181    
182    /* Copy packet in buffer */
183    jitter->buf[i]=speex_alloc(packet->len);
184    for (j=0;j<packet->len;j++)
185       jitter->buf[i][j]=packet->data[j];
186    jitter->timestamp[i]=packet->timestamp;
187    jitter->span[i]=packet->span;
188    jitter->len[i]=packet->len;
189    
190    /* Adjust the buffer size depending on network conditions */
191    arrival_margin = (packet->timestamp - jitter->current_timestamp) - jitter->buffer_margin*jitter->tick_size;
192    
193    if (arrival_margin >= -LATE_BINS*jitter->tick_size)
194    {
195       spx_int32_t int_margin;
196       for (i=0;i<MAX_MARGIN;i++)
197       {
198          jitter->shortterm_margin[i] *= .98;
199          jitter->longterm_margin[i] *= .995;
200       }
201       int_margin = LATE_BINS + arrival_margin/jitter->tick_size;
202       if (int_margin>MAX_MARGIN-1)
203          int_margin = MAX_MARGIN-1;
204       if (int_margin>=0)
205       {
206          jitter->shortterm_margin[int_margin] += .02;
207          jitter->longterm_margin[int_margin] += .005;
208       }
209    } else {
210       
211       /*fprintf (stderr, "way too late = %d\n", arrival_margin);*/
212       if (jitter->lost_count>20)
213       {
214          jitter_buffer_reset(jitter);
215       }
216    }
217 #if 0 /* Enable to check how much is being buffered */
218    if (rand()%1000==0)
219    {
220       int count = 0;
221       for (j=0;j<SPEEX_JITTER_MAX_BUFFER_SIZE;j++)
222       {
223          if (jitter->buf[j])
224             count++;
225       }
226       fprintf (stderr, "buffer_size = %d\n", count);
227    }
228 #endif
229 }
230
231 /** Get one packet from the jitter buffer */
232 int jitter_buffer_get(JitterBuffer *jitter, JitterBufferPacket *packet, spx_uint32_t *start_offset)
233 {
234    int i, j;
235    float late_ratio_short;
236    float late_ratio_long;
237    float ontime_ratio_short;
238    float ontime_ratio_long;
239    float early_ratio_short;
240    float early_ratio_long;
241    int chunk_size;
242    int incomplete = 0;
243    
244    if (LT32(jitter->current_timestamp+jitter->tick_size, jitter->pointer_timestamp))
245    {
246       jitter->current_timestamp = jitter->pointer_timestamp;
247       speex_warning("did you forget to call jitter_buffer_tick() by any chance?");
248    }
249    /*fprintf (stderr, "get packet %d %d\n", jitter->pointer_timestamp, jitter->current_timestamp);*/
250
251    /* FIXME: This should be only what remaining of the current tick */
252    chunk_size = jitter->tick_size;
253    
254    /* Compiling arrival statistics */
255    
256    late_ratio_short = 0;
257    late_ratio_long = 0;
258    for (i=0;i<LATE_BINS;i++)
259    {
260       late_ratio_short += jitter->shortterm_margin[i];
261       late_ratio_long += jitter->longterm_margin[i];
262    }
263    ontime_ratio_short = jitter->shortterm_margin[LATE_BINS];
264    ontime_ratio_long = jitter->longterm_margin[LATE_BINS];
265    early_ratio_short = early_ratio_long = 0;
266    for (i=LATE_BINS+1;i<MAX_MARGIN;i++)
267    {
268       early_ratio_short += jitter->shortterm_margin[i];
269       early_ratio_long += jitter->longterm_margin[i];
270    }
271    if (0&&jitter->pointer_timestamp%1000==0)
272    {
273       /*fprintf (stderr, "%f %f %f %f %f %f\n", early_ratio_short, early_ratio_long, ontime_ratio_short, ontime_ratio_long, late_ratio_short, late_ratio_long);*/
274       /*fprintf (stderr, "%f %f\n", early_ratio_short + ontime_ratio_short + late_ratio_short, early_ratio_long + ontime_ratio_long + late_ratio_long);*/
275    }
276    
277    /* Adjusting the buffering */
278    
279    if (late_ratio_short > .1 || late_ratio_long > .03)
280    {
281       /* If too many packets are arriving late */
282       jitter->shortterm_margin[MAX_MARGIN-1] += jitter->shortterm_margin[MAX_MARGIN-2];
283       jitter->longterm_margin[MAX_MARGIN-1] += jitter->longterm_margin[MAX_MARGIN-2];
284       for (i=MAX_MARGIN-3;i>=0;i--)
285       {
286          jitter->shortterm_margin[i+1] = jitter->shortterm_margin[i];
287          jitter->longterm_margin[i+1] = jitter->longterm_margin[i];         
288       }
289       jitter->shortterm_margin[0] = 0;
290       jitter->longterm_margin[0] = 0;            
291       jitter->pointer_timestamp -= jitter->tick_size;
292       jitter->current_timestamp -= jitter->tick_size;
293       /*fprintf (stderr, "i");*/
294       /*fprintf (stderr, "interpolate (getting some slack)\n");*/
295    } else if (late_ratio_short + ontime_ratio_short < .005 && late_ratio_long + ontime_ratio_long < .01 && early_ratio_short > .8)
296    {
297       /* Many frames arriving early */
298       jitter->shortterm_margin[0] += jitter->shortterm_margin[1];
299       jitter->longterm_margin[0] += jitter->longterm_margin[1];
300       for (i=1;i<MAX_MARGIN-1;i++)
301       {
302          jitter->shortterm_margin[i] = jitter->shortterm_margin[i+1];
303          jitter->longterm_margin[i] = jitter->longterm_margin[i+1];         
304       }
305       jitter->shortterm_margin[MAX_MARGIN-1] = 0;
306       jitter->longterm_margin[MAX_MARGIN-1] = 0;      
307       /*fprintf (stderr, "drop frame\n");*/
308       /*fprintf (stderr, "d");*/
309       jitter->pointer_timestamp += jitter->tick_size;
310       jitter->current_timestamp += jitter->tick_size;
311       /*fprintf (stderr, "dropping packet (getting more aggressive)\n");*/
312    }
313    
314    /* Searching for the packet that fits best */
315    
316    /* Search the buffer for a packet with the right timestamp and spanning the whole current chunk */
317    for (i=0;i<SPEEX_JITTER_MAX_BUFFER_SIZE;i++)
318    {
319       if (jitter->buf[i] && jitter->timestamp[i]==jitter->pointer_timestamp && GE32(jitter->timestamp[i]+jitter->span[i],jitter->pointer_timestamp+chunk_size))
320          break;
321    }
322    
323    /* If no match, try for an "older" packet that still spans (fully) the current chunk */
324    if (i==SPEEX_JITTER_MAX_BUFFER_SIZE)
325    {
326       for (i=0;i<SPEEX_JITTER_MAX_BUFFER_SIZE;i++)
327       {
328          if (jitter->buf[i] && jitter->timestamp[i]<=jitter->pointer_timestamp && GE32(jitter->timestamp[i]+jitter->span[i],jitter->pointer_timestamp+chunk_size))
329             break;
330       }
331    }
332    
333    /* If still no match, try for an "older" packet that spans part of the current chunk */
334    if (i==SPEEX_JITTER_MAX_BUFFER_SIZE)
335    {
336       for (i=0;i<SPEEX_JITTER_MAX_BUFFER_SIZE;i++)
337       {
338          if (jitter->buf[i] && jitter->timestamp[i]<=jitter->pointer_timestamp && GT32(jitter->timestamp[i]+jitter->span[i],jitter->pointer_timestamp))
339             break;
340       }
341    }
342    
343    /* If still no match, try for earliest packet possible */
344    if (i==SPEEX_JITTER_MAX_BUFFER_SIZE)
345    {
346       int found = 0;
347       spx_uint32_t best_time=0;
348       int best_span=0;
349       int besti=0;
350       for (i=0;i<SPEEX_JITTER_MAX_BUFFER_SIZE;i++)
351       {
352          /* check if packet starts within current chunk */
353          if (jitter->buf[i] && LT32(jitter->timestamp[i],jitter->pointer_timestamp+chunk_size) && GE32(jitter->timestamp[i],jitter->pointer_timestamp))
354          {
355             if (!found || LT32(jitter->timestamp[i],best_time) || (jitter->timestamp[i]==best_time && GT32(jitter->span[i],best_span)))
356             {
357                best_time = jitter->timestamp[i];
358                best_span = jitter->span[i];
359                besti = i;
360                found = 1;
361             }
362          }
363       }
364       if (found)
365       {
366          i=besti;
367          incomplete = 1;
368          /*fprintf (stderr, "incomplete: %d %d %d %d\n", jitter->timestamp[i], jitter->pointer_timestamp, chunk_size, jitter->span[i]);*/
369       }
370    }
371
372    /* If we find something */
373    if (i!=SPEEX_JITTER_MAX_BUFFER_SIZE)
374    {
375       /* We (obviously) haven't lost this packet */
376       jitter->lost_count = 0;
377       jitter->loss_rate = .999*jitter->loss_rate;
378       /* Check for potential overflow */
379       packet->len = jitter->len[i];
380       /* Copy packet */
381       for (j=0;j<packet->len;j++)
382          packet->data[j] = jitter->buf[i][j];
383       /* Remove packet */
384       speex_free(jitter->buf[i]);
385       jitter->buf[i] = NULL;
386       /* Set timestamp and span (if requested) */
387       if (start_offset)
388          *start_offset = jitter->timestamp[i]-jitter->pointer_timestamp;
389       packet->timestamp = jitter->timestamp[i];
390       packet->span = jitter->span[i];
391       /* Point at the end of the current packet */
392       jitter->pointer_timestamp = jitter->timestamp[i]+jitter->span[i];
393       if (incomplete)
394          return JITTER_BUFFER_INCOMPLETE;
395       else
396          return JITTER_BUFFER_OK;
397    }
398    
399    
400    /* If we haven't found anything worth returning */
401    /*fprintf (stderr, "not found\n");*/
402    jitter->lost_count++;
403    /*fprintf (stderr, "m");*/
404    /*fprintf (stderr, "lost_count = %d\n", jitter->lost_count);*/
405    jitter->loss_rate = .999*jitter->loss_rate + .001;
406    if (start_offset)
407       *start_offset = 0;
408    packet->timestamp = jitter->pointer_timestamp;
409    packet->span = jitter->tick_size;
410    jitter->pointer_timestamp += chunk_size;
411    packet->len = 0;
412    return JITTER_BUFFER_MISSING;
413
414 }
415
416 /** Get pointer timestamp of jitter buffer */
417 int jitter_buffer_get_pointer_timestamp(JitterBuffer *jitter)
418 {
419    return jitter->pointer_timestamp;
420 }
421
422 void jitter_buffer_tick(JitterBuffer *jitter)
423 {
424    jitter->current_timestamp += jitter->tick_size;
425 }
426
427
428
429
430
431 void speex_jitter_init(SpeexJitter *jitter, void *decoder, int sampling_rate)
432 {
433    jitter->dec = decoder;
434    speex_decoder_ctl(decoder, SPEEX_GET_FRAME_SIZE, &jitter->frame_size);
435
436    jitter->packets = jitter_buffer_init(jitter->frame_size);
437
438    speex_bits_init(&jitter->current_packet);
439    jitter->valid_bits = 0;
440
441 }
442
443 void speex_jitter_destroy(SpeexJitter *jitter)
444 {
445    jitter_buffer_destroy(jitter->packets);
446    speex_bits_destroy(&jitter->current_packet);
447 }
448
449 void speex_jitter_put(SpeexJitter *jitter, char *packet, int len, int timestamp)
450 {
451    JitterBufferPacket p;
452    p.data = packet;
453    p.len = len;
454    p.timestamp = timestamp;
455    p.span = jitter->frame_size;
456    jitter_buffer_put(jitter->packets, &p);
457 }
458
459 void speex_jitter_get(SpeexJitter *jitter, short *out, int *current_timestamp)
460 {
461    int i;
462    int ret;
463    char data[2048];
464    JitterBufferPacket packet;
465    packet.data = data;
466    
467    if (jitter->valid_bits)
468    {
469       /* Try decoding last received packet */
470       ret = speex_decode_int(jitter->dec, &jitter->current_packet, out);
471       if (ret == 0)
472       {
473          jitter_buffer_tick(jitter->packets);
474          return;
475       } else {
476          jitter->valid_bits = 0;
477       }
478    }
479
480    ret = jitter_buffer_get(jitter->packets, &packet, NULL);
481    
482    if (ret != JITTER_BUFFER_OK)
483    {
484       /* No packet found */
485
486       /*fprintf (stderr, "lost/late frame\n");*/
487       /*Packet is late or lost*/
488       speex_decode_int(jitter->dec, NULL, out);
489    } else {
490       speex_bits_read_from(&jitter->current_packet, packet.data, packet.len);
491       /* Decode packet */
492       ret = speex_decode_int(jitter->dec, &jitter->current_packet, out);
493       if (ret == 0)
494       {
495          jitter->valid_bits = 1;
496       } else {
497          /* Error while decoding */
498          for (i=0;i<jitter->frame_size;i++)
499             out[i]=0;
500       }
501    }
502    jitter_buffer_tick(jitter->packets);
503 }
504
505 int speex_jitter_get_pointer_timestamp(SpeexJitter *jitter)
506 {
507    return jitter_buffer_get_pointer_timestamp(jitter->packets);
508 }