Add silence detection to app_record
[asterisk/asterisk.git] / dsp.c
1 /*
2  * Asterisk -- A telephony toolkit for Linux.
3  *
4  * Convenience Signal Processing routines
5  * 
6  * Copyright (C) 2002, Digium
7  *
8  * Mark Spencer <markster@linux-support.net>
9  *
10  * This program is free software, distributed under the terms of
11  * the GNU General Public License.
12  *
13  * Goertzel routines are borrowed from Steve Underwood's tremendous work on the
14  * DTMF detector.
15  *
16  */
17
18 /* Some routines from tone_detect.c by Steven Underwood as published under the zapata library */
19 /*
20         tone_detect.c - General telephony tone detection, and specific
21                         detection of DTMF.
22
23         Copyright (C) 2001  Steve Underwood <steveu@coppice.org>
24
25         Despite my general liking of the GPL, I place this code in the
26         public domain for the benefit of all mankind - even the slimy
27         ones who might try to proprietize my work and use it to my
28         detriment.
29 */
30
31 #include <sys/types.h>
32 #include <asterisk/frame.h>
33 #include <asterisk/channel.h>
34 #include <asterisk/channel_pvt.h>
35 #include <asterisk/logger.h>
36 #include <asterisk/dsp.h>
37 #include <asterisk/ulaw.h>
38 #include <asterisk/alaw.h>
39 #include <stdlib.h>
40 #include <unistd.h>
41 #include <string.h>
42 #include <math.h>
43 #include <errno.h>
44 #include <stdio.h>
45
46 #define DEFAULT_THRESHOLD 1024
47
48 #define BUSY_THRESHOLD  100             /* Max number of ms difference between max and min times in busy */
49 #define BUSY_MIN                80              /* Busy must be at least 80 ms in half-cadence */
50 #define BUSY_MAX                1100    /* Busy can't be longer than 1100 ms in half-cadence */
51
52 /* Remember last 3 units */
53 #define DSP_HISTORY 5
54
55 /* Number of goertzels for progress detect */
56 #define GSAMP_SIZE 183
57
58 #define HZ_350  0
59 #define HZ_440  1
60 #define HZ_480  2
61 #define HZ_620  3
62 #define HZ_950  4
63 #define HZ_1400 5
64 #define HZ_1800 6
65
66 #define TONE_THRESH 10.0        /* How much louder the tone should be than channel energy */
67 #define TONE_MIN_THRESH 1e8     /* How much tone there should be at least to attempt */
68 #define COUNT_THRESH  3         /* Need at least 50ms of stuff to count it */
69
70 #define TONE_STATE_SILENCE  0
71 #define TONE_STATE_RINGING  1 
72 #define TONE_STATE_DIALTONE 2
73 #define TONE_STATE_TALKING  3
74 #define TONE_STATE_BUSY     4
75 #define TONE_STATE_SPECIAL1     5
76 #define TONE_STATE_SPECIAL2 6
77 #define TONE_STATE_SPECIAL3 7
78
79 #define MAX_DTMF_DIGITS 128
80
81 /* Basic DTMF specs:
82  *
83  * Minimum tone on = 40ms
84  * Minimum tone off = 50ms
85  * Maximum digit rate = 10 per second
86  * Normal twist <= 8dB accepted
87  * Reverse twist <= 4dB accepted
88  * S/N >= 15dB will detect OK
89  * Attenuation <= 26dB will detect OK
90  * Frequency tolerance +- 1.5% will detect, +-3.5% will reject
91  */
92
93 #define DTMF_THRESHOLD              8.0e7
94 #define FAX_THRESHOLD              8.0e7
95 #define FAX_2ND_HARMONIC                2.0     /* 4dB */
96 #define DTMF_NORMAL_TWIST           6.3     /* 8dB */
97 #define DTMF_REVERSE_TWIST          ((digitmode & DSP_DIGITMODE_RELAXDTMF) ? 4.0 : 2.5)     /* 4dB normal */
98 #define DTMF_RELATIVE_PEAK_ROW      6.3     /* 8dB */
99 #define DTMF_RELATIVE_PEAK_COL      6.3     /* 8dB */
100 #define DTMF_2ND_HARMONIC_ROW       ((digitmode & DSP_DIGITMODE_RELAXDTMF) ? 1.7 : 2.5)     /* 4dB normal */
101 #define DTMF_2ND_HARMONIC_COL       63.1    /* 18dB */
102
103 #define MF_THRESHOLD              8.0e7
104 #define MF_NORMAL_TWIST           5.3     /* 8dB */
105 #define MF_REVERSE_TWIST          4.0     /* was 2.5 */
106 #define MF_RELATIVE_PEAK      5.3     /* 8dB */
107 #define MF_2ND_HARMONIC       1.7 /* was 2.5  */
108
109 typedef struct {
110         float v2;
111         float v3;
112         float fac;
113 } goertzel_state_t;
114
115 typedef struct
116 {
117     int hit1;
118     int hit2;
119     int hit3;
120     int hit4;
121     int mhit;
122
123     goertzel_state_t row_out[4];
124     goertzel_state_t col_out[4];
125     goertzel_state_t row_out2nd[4];
126     goertzel_state_t col_out2nd[4];
127         goertzel_state_t fax_tone;
128         goertzel_state_t fax_tone2nd;
129     float energy;
130     
131     int current_sample;
132     char digits[MAX_DTMF_DIGITS + 1];
133     int current_digits;
134     int detected_digits;
135     int lost_digits;
136     int digit_hits[16];
137         int fax_hits;
138 } dtmf_detect_state_t;
139
140 typedef struct
141 {
142     int hit1;
143     int hit2;
144     int hit3;
145     int hit4;
146     int mhit;
147
148     goertzel_state_t tone_out[6];
149     goertzel_state_t tone_out2nd[6];
150     float energy;
151     
152     int current_sample;
153     char digits[MAX_DTMF_DIGITS + 1];
154     int current_digits;
155     int detected_digits;
156     int lost_digits;
157         int fax_hits;
158 } mf_detect_state_t;
159
160 static float dtmf_row[] =
161 {
162      697.0,  770.0,  852.0,  941.0
163 };
164 static float dtmf_col[] =
165 {
166     1209.0, 1336.0, 1477.0, 1633.0
167 };
168
169 static float mf_tones[] =
170 {
171         700.0, 900.0, 1100.0, 1300.0, 1500.0, 1700.0
172 };
173
174 static float fax_freq = 1100.0;
175
176 static char dtmf_positions[] = "123A" "456B" "789C" "*0#D";
177
178 static char mf_hit[6][6] = {
179         /*  700 + */ {   0, '1', '2', '4', '7', 'C' },
180         /*  900 + */ { '1',   0, '3', '5', '8', 'A' },
181         /* 1100 + */ { '2', '3',   0, '6', '9', '*' },
182         /* 1300 + */ { '4', '5', '6',   0, '0', 'B' },
183         /* 1500 + */ { '7', '8', '9', '0',  0, '#' },
184         /* 1700 + */ { 'C', 'A', '*', 'B', '#',  0  },
185 };
186
187 static inline void goertzel_sample(goertzel_state_t *s, short sample)
188 {
189         float v1;
190         float fsamp  = sample;
191         v1 = s->v2;
192         s->v2 = s->v3;
193         s->v3 = s->fac * s->v2 - v1 + fsamp;
194 }
195
196 static inline void goertzel_update(goertzel_state_t *s, short *samps, int count)
197 {
198         int i;
199         for (i=0;i<count;i++) 
200                 goertzel_sample(s, samps[i]);
201 }
202
203
204 static inline float goertzel_result(goertzel_state_t *s)
205 {
206         return s->v3 * s->v3 + s->v2 * s->v2 - s->v2 * s->v3 * s->fac;
207 }
208
209 static inline void goertzel_init(goertzel_state_t *s, float freq)
210 {
211         s->v2 = s->v3 = 0.0;
212         s->fac = 2.0 * cos(2.0 * M_PI * (freq / 8000.0));
213 }
214
215 static inline void goertzel_reset(goertzel_state_t *s)
216 {
217         s->v2 = s->v3 = 0.0;
218 }
219
220 struct ast_dsp {
221         struct ast_frame f;
222         int threshold;
223         int totalsilence;
224         int totalnoise;
225         int features;
226         int busymaybe;
227         int busycount;
228         int historicnoise[DSP_HISTORY];
229         int historicsilence[DSP_HISTORY];
230         goertzel_state_t freqs[7];
231         int gsamps;
232         int tstate;
233         int tcount;
234         int digitmode;
235         int thinkdigit;
236         float genergy;
237         union {
238                 dtmf_detect_state_t dtmf;
239                 mf_detect_state_t mf;
240         } td;
241 };
242
243 static void ast_dtmf_detect_init (dtmf_detect_state_t *s)
244 {
245     int i;
246
247     s->hit1 = 
248     s->hit2 = 0;
249
250     for (i = 0;  i < 4;  i++)
251     {
252     
253                 goertzel_init (&s->row_out[i], dtmf_row[i]);
254         goertzel_init (&s->col_out[i], dtmf_col[i]);
255         goertzel_init (&s->row_out2nd[i], dtmf_row[i] * 2.0);
256         goertzel_init (&s->col_out2nd[i], dtmf_col[i] * 2.0);
257         
258                 s->energy = 0.0;
259     }
260
261         /* Same for the fax dector */
262     goertzel_init (&s->fax_tone, fax_freq);
263
264         /* Same for the fax dector 2nd harmonic */
265     goertzel_init (&s->fax_tone2nd, fax_freq * 2.0);
266         
267     s->current_sample = 0;
268     s->detected_digits = 0;
269         s->current_digits = 0;
270         memset(&s->digits, 0, sizeof(s->digits));
271     s->lost_digits = 0;
272     s->digits[0] = '\0';
273     s->mhit = 0;
274 }
275
276 static void ast_mf_detect_init (mf_detect_state_t *s)
277 {
278     int i;
279
280     s->hit1 = 
281     s->hit2 = 0;
282
283     for (i = 0;  i < 6;  i++)
284     {
285     
286                 goertzel_init (&s->tone_out[i], mf_tones[i]);
287         goertzel_init (&s->tone_out2nd[i], mf_tones[i] * 2.0);
288         
289                 s->energy = 0.0;
290     }
291
292         s->current_digits = 0;
293         memset(&s->digits, 0, sizeof(s->digits));
294     s->current_sample = 0;
295     s->detected_digits = 0;
296     s->lost_digits = 0;
297     s->digits[0] = '\0';
298     s->mhit = 0;
299 }
300
301 static int dtmf_detect (dtmf_detect_state_t *s,
302                  int16_t amp[],
303                  int samples, 
304                  int digitmode, int *writeback)
305 {
306
307     float row_energy[4];
308     float col_energy[4];
309     float fax_energy;
310     float fax_energy_2nd;
311     float famp;
312     float v1;
313     int i;
314     int j;
315     int sample;
316     int best_row;
317     int best_col;
318     int hit;
319     int limit;
320
321     hit = 0;
322     for (sample = 0;  sample < samples;  sample = limit)
323     {
324         /* 102 is optimised to meet the DTMF specs. */
325         if ((samples - sample) >= (102 - s->current_sample))
326             limit = sample + (102 - s->current_sample);
327         else
328             limit = samples;
329 #if defined(USE_3DNOW)
330         _dtmf_goertzel_update (s->row_out, amp + sample, limit - sample);
331         _dtmf_goertzel_update (s->col_out, amp + sample, limit - sample);
332         _dtmf_goertzel_update (s->row_out2nd, amp + sample, limit2 - sample);
333         _dtmf_goertzel_update (s->col_out2nd, amp + sample, limit2 - sample);
334                 /* XXX Need to fax detect for 3dnow too XXX */
335                 #warning "Fax Support Broken"
336 #else
337         /* The following unrolled loop takes only 35% (rough estimate) of the 
338            time of a rolled loop on the machine on which it was developed */
339         for (j = sample;  j < limit;  j++)
340         {
341             famp = amp[j];
342             
343             s->energy += famp*famp;
344             
345             /* With GCC 2.95, the following unrolled code seems to take about 35%
346                (rough estimate) as long as a neat little 0-3 loop */
347             v1 = s->row_out[0].v2;
348             s->row_out[0].v2 = s->row_out[0].v3;
349             s->row_out[0].v3 = s->row_out[0].fac*s->row_out[0].v2 - v1 + famp;
350     
351             v1 = s->col_out[0].v2;
352             s->col_out[0].v2 = s->col_out[0].v3;
353             s->col_out[0].v3 = s->col_out[0].fac*s->col_out[0].v2 - v1 + famp;
354     
355             v1 = s->row_out[1].v2;
356             s->row_out[1].v2 = s->row_out[1].v3;
357             s->row_out[1].v3 = s->row_out[1].fac*s->row_out[1].v2 - v1 + famp;
358     
359             v1 = s->col_out[1].v2;
360             s->col_out[1].v2 = s->col_out[1].v3;
361             s->col_out[1].v3 = s->col_out[1].fac*s->col_out[1].v2 - v1 + famp;
362     
363             v1 = s->row_out[2].v2;
364             s->row_out[2].v2 = s->row_out[2].v3;
365             s->row_out[2].v3 = s->row_out[2].fac*s->row_out[2].v2 - v1 + famp;
366     
367             v1 = s->col_out[2].v2;
368             s->col_out[2].v2 = s->col_out[2].v3;
369             s->col_out[2].v3 = s->col_out[2].fac*s->col_out[2].v2 - v1 + famp;
370     
371             v1 = s->row_out[3].v2;
372             s->row_out[3].v2 = s->row_out[3].v3;
373             s->row_out[3].v3 = s->row_out[3].fac*s->row_out[3].v2 - v1 + famp;
374
375             v1 = s->col_out[3].v2;
376             s->col_out[3].v2 = s->col_out[3].v3;
377             s->col_out[3].v3 = s->col_out[3].fac*s->col_out[3].v2 - v1 + famp;
378
379             v1 = s->col_out2nd[0].v2;
380             s->col_out2nd[0].v2 = s->col_out2nd[0].v3;
381             s->col_out2nd[0].v3 = s->col_out2nd[0].fac*s->col_out2nd[0].v2 - v1 + famp;
382         
383             v1 = s->row_out2nd[0].v2;
384             s->row_out2nd[0].v2 = s->row_out2nd[0].v3;
385             s->row_out2nd[0].v3 = s->row_out2nd[0].fac*s->row_out2nd[0].v2 - v1 + famp;
386         
387             v1 = s->col_out2nd[1].v2;
388             s->col_out2nd[1].v2 = s->col_out2nd[1].v3;
389             s->col_out2nd[1].v3 = s->col_out2nd[1].fac*s->col_out2nd[1].v2 - v1 + famp;
390     
391             v1 = s->row_out2nd[1].v2;
392             s->row_out2nd[1].v2 = s->row_out2nd[1].v3;
393             s->row_out2nd[1].v3 = s->row_out2nd[1].fac*s->row_out2nd[1].v2 - v1 + famp;
394         
395             v1 = s->col_out2nd[2].v2;
396             s->col_out2nd[2].v2 = s->col_out2nd[2].v3;
397             s->col_out2nd[2].v3 = s->col_out2nd[2].fac*s->col_out2nd[2].v2 - v1 + famp;
398         
399             v1 = s->row_out2nd[2].v2;
400             s->row_out2nd[2].v2 = s->row_out2nd[2].v3;
401             s->row_out2nd[2].v3 = s->row_out2nd[2].fac*s->row_out2nd[2].v2 - v1 + famp;
402         
403             v1 = s->col_out2nd[3].v2;
404             s->col_out2nd[3].v2 = s->col_out2nd[3].v3;
405             s->col_out2nd[3].v3 = s->col_out2nd[3].fac*s->col_out2nd[3].v2 - v1 + famp;
406         
407             v1 = s->row_out2nd[3].v2;
408             s->row_out2nd[3].v2 = s->row_out2nd[3].v3;
409             s->row_out2nd[3].v3 = s->row_out2nd[3].fac*s->row_out2nd[3].v2 - v1 + famp;
410
411                         /* Update fax tone */
412             v1 = s->fax_tone.v2;
413             s->fax_tone.v2 = s->fax_tone.v3;
414             s->fax_tone.v3 = s->fax_tone.fac*s->fax_tone.v2 - v1 + famp;
415
416             v1 = s->fax_tone.v2;
417             s->fax_tone2nd.v2 = s->fax_tone2nd.v3;
418             s->fax_tone2nd.v3 = s->fax_tone2nd.fac*s->fax_tone2nd.v2 - v1 + famp;
419         }
420 #endif
421         s->current_sample += (limit - sample);
422         if (s->current_sample < 102) {
423                         if (hit && !((digitmode & DSP_DIGITMODE_NOQUELCH))) {
424                                 /* If we had a hit last time, go ahead and clear this out since likely it
425                                    will be another hit */
426                                 for (i=sample;i<limit;i++) 
427                                         amp[i] = 0;
428                                 *writeback = 1;
429                         }
430             continue;
431                 }
432
433                 /* Detect the fax energy, too */
434                 fax_energy = goertzel_result(&s->fax_tone);
435                 
436         /* We are at the end of a DTMF detection block */
437         /* Find the peak row and the peak column */
438         row_energy[0] = goertzel_result (&s->row_out[0]);
439         col_energy[0] = goertzel_result (&s->col_out[0]);
440
441         for (best_row = best_col = 0, i = 1;  i < 4;  i++)
442         {
443             row_energy[i] = goertzel_result (&s->row_out[i]);
444             if (row_energy[i] > row_energy[best_row])
445                 best_row = i;
446             col_energy[i] = goertzel_result (&s->col_out[i]);
447             if (col_energy[i] > col_energy[best_col])
448                 best_col = i;
449         }
450         hit = 0;
451         /* Basic signal level test and the twist test */
452         if (row_energy[best_row] >= DTMF_THRESHOLD
453             &&
454             col_energy[best_col] >= DTMF_THRESHOLD
455             &&
456             col_energy[best_col] < row_energy[best_row]*DTMF_REVERSE_TWIST
457             &&
458             col_energy[best_col]*DTMF_NORMAL_TWIST > row_energy[best_row])
459         {
460             /* Relative peak test */
461             for (i = 0;  i < 4;  i++)
462             {
463                 if ((i != best_col  &&  col_energy[i]*DTMF_RELATIVE_PEAK_COL > col_energy[best_col])
464                     ||
465                     (i != best_row  &&  row_energy[i]*DTMF_RELATIVE_PEAK_ROW > row_energy[best_row]))
466                 {
467                     break;
468                 }
469             }
470             /* ... and second harmonic test */
471             if (i >= 4
472                 &&
473                 (row_energy[best_row] + col_energy[best_col]) > 42.0*s->energy
474                 &&
475                 goertzel_result (&s->col_out2nd[best_col])*DTMF_2ND_HARMONIC_COL < col_energy[best_col]
476                 &&
477                 goertzel_result (&s->row_out2nd[best_row])*DTMF_2ND_HARMONIC_ROW < row_energy[best_row])
478             {
479                                 /* Got a hit */
480                 hit = dtmf_positions[(best_row << 2) + best_col];
481                                 if (!(digitmode & DSP_DIGITMODE_NOQUELCH)) {
482                                         /* Zero out frame data if this is part DTMF */
483                                         for (i=sample;i<limit;i++) 
484                                                 amp[i] = 0;
485                                         *writeback = 1;
486                                 }
487                 /* Look for two successive similar results */
488                 /* The logic in the next test is:
489                    We need two successive identical clean detects, with
490                    something different preceeding it. This can work with
491                    back to back differing digits. More importantly, it
492                    can work with nasty phones that give a very wobbly start
493                    to a digit. */
494                 if (hit == s->hit3  &&  s->hit3 != s->hit2)
495                 {
496                     s->mhit = hit;
497                     s->digit_hits[(best_row << 2) + best_col]++;
498                     s->detected_digits++;
499                     if (s->current_digits < MAX_DTMF_DIGITS)
500                     {
501                         s->digits[s->current_digits++] = hit;
502                         s->digits[s->current_digits] = '\0';
503                     }
504                     else
505                     {
506                         s->lost_digits++;
507                     }
508                 }
509             }
510         } 
511                 if (!hit && (fax_energy >= FAX_THRESHOLD) && (fax_energy > s->energy * 21.0)) {
512                                 fax_energy_2nd = goertzel_result(&s->fax_tone2nd);
513                                 if (fax_energy_2nd * FAX_2ND_HARMONIC < fax_energy) {
514 #if 0
515                                         printf("Fax energy/Second Harmonic: %f/%f\n", fax_energy, fax_energy_2nd);
516 #endif                                  
517                                         /* XXX Probably need better checking than just this the energy XXX */
518                                         hit = 'f';
519                                         s->fax_hits++;
520                                 } /* Don't reset fax hits counter */
521                 } else {
522                         if (s->fax_hits > 5) {
523                                  hit = 'f';
524                                  s->mhit = 'f';
525                      s->detected_digits++;
526                      if (s->current_digits < MAX_DTMF_DIGITS)
527                      {
528                           s->digits[s->current_digits++] = hit;
529                           s->digits[s->current_digits] = '\0';
530                      }
531                      else
532                      {
533                            s->lost_digits++;
534                      }
535                         }
536                         s->fax_hits = 0;
537                 }
538         s->hit1 = s->hit2;
539         s->hit2 = s->hit3;
540         s->hit3 = hit;
541         /* Reinitialise the detector for the next block */
542         for (i = 0;  i < 4;  i++)
543         {
544             goertzel_reset(&s->row_out[i]);
545             goertzel_reset(&s->col_out[i]);
546             goertzel_reset(&s->row_out2nd[i]);
547             goertzel_reset(&s->col_out2nd[i]);
548         }
549         goertzel_reset (&s->fax_tone);
550         goertzel_reset (&s->fax_tone2nd);
551                 s->energy = 0.0;
552         s->current_sample = 0;
553     }
554     if ((!s->mhit) || (s->mhit != hit))
555     {
556         s->mhit = 0;
557         return(0);
558     }
559     return (hit);
560 }
561
562 /* MF goertzel size */
563 #define MF_GSIZE 160
564
565 static int mf_detect (mf_detect_state_t *s,
566                  int16_t amp[],
567                  int samples, 
568                  int digitmode, int *writeback)
569 {
570
571     float tone_energy[6];
572     float famp;
573     float v1;
574     int i;
575     int j;
576     int sample;
577     int best1;
578     int best2;
579         float max;
580     int hit;
581     int limit;
582         int sofarsogood;
583
584     hit = 0;
585     for (sample = 0;  sample < samples;  sample = limit)
586     {
587         /* 80 is optimised to meet the MF specs. */
588         if ((samples - sample) >= (MF_GSIZE - s->current_sample))
589             limit = sample + (MF_GSIZE - s->current_sample);
590         else
591             limit = samples;
592 #if defined(USE_3DNOW)
593         _dtmf_goertzel_update (s->row_out, amp + sample, limit - sample);
594         _dtmf_goertzel_update (s->col_out, amp + sample, limit - sample);
595         _dtmf_goertzel_update (s->row_out2nd, amp + sample, limit2 - sample);
596         _dtmf_goertzel_update (s->col_out2nd, amp + sample, limit2 - sample);
597                 /* XXX Need to fax detect for 3dnow too XXX */
598                 #warning "Fax Support Broken"
599 #else
600         /* The following unrolled loop takes only 35% (rough estimate) of the 
601            time of a rolled loop on the machine on which it was developed */
602         for (j = sample;  j < limit;  j++)
603         {
604             famp = amp[j];
605             
606             s->energy += famp*famp;
607             
608             /* With GCC 2.95, the following unrolled code seems to take about 35%
609                (rough estimate) as long as a neat little 0-3 loop */
610             v1 = s->tone_out[0].v2;
611             s->tone_out[0].v2 = s->tone_out[0].v3;
612             s->tone_out[0].v3 = s->tone_out[0].fac*s->tone_out[0].v2 - v1 + famp;
613
614             v1 = s->tone_out[1].v2;
615             s->tone_out[1].v2 = s->tone_out[1].v3;
616             s->tone_out[1].v3 = s->tone_out[1].fac*s->tone_out[1].v2 - v1 + famp;
617     
618             v1 = s->tone_out[2].v2;
619             s->tone_out[2].v2 = s->tone_out[2].v3;
620             s->tone_out[2].v3 = s->tone_out[2].fac*s->tone_out[2].v2 - v1 + famp;
621     
622             v1 = s->tone_out[3].v2;
623             s->tone_out[3].v2 = s->tone_out[3].v3;
624             s->tone_out[3].v3 = s->tone_out[3].fac*s->tone_out[3].v2 - v1 + famp;
625
626             v1 = s->tone_out[4].v2;
627             s->tone_out[4].v2 = s->tone_out[4].v3;
628             s->tone_out[4].v3 = s->tone_out[4].fac*s->tone_out[4].v2 - v1 + famp;
629
630             v1 = s->tone_out[5].v2;
631             s->tone_out[5].v2 = s->tone_out[5].v3;
632             s->tone_out[5].v3 = s->tone_out[5].fac*s->tone_out[5].v2 - v1 + famp;
633
634             v1 = s->tone_out2nd[0].v2;
635             s->tone_out2nd[0].v2 = s->tone_out2nd[0].v3;
636             s->tone_out2nd[0].v3 = s->tone_out2nd[0].fac*s->tone_out2nd[0].v2 - v1 + famp;
637         
638             v1 = s->tone_out2nd[1].v2;
639             s->tone_out2nd[1].v2 = s->tone_out2nd[1].v3;
640             s->tone_out2nd[1].v3 = s->tone_out2nd[1].fac*s->tone_out2nd[1].v2 - v1 + famp;
641         
642             v1 = s->tone_out2nd[2].v2;
643             s->tone_out2nd[2].v2 = s->tone_out2nd[2].v3;
644             s->tone_out2nd[2].v3 = s->tone_out2nd[2].fac*s->tone_out2nd[2].v2 - v1 + famp;
645         
646             v1 = s->tone_out2nd[3].v2;
647             s->tone_out2nd[3].v2 = s->tone_out2nd[3].v3;
648             s->tone_out2nd[3].v3 = s->tone_out2nd[3].fac*s->tone_out2nd[3].v2 - v1 + famp;
649
650             v1 = s->tone_out2nd[4].v2;
651             s->tone_out2nd[4].v2 = s->tone_out2nd[4].v3;
652             s->tone_out2nd[4].v3 = s->tone_out2nd[4].fac*s->tone_out2nd[2].v2 - v1 + famp;
653         
654             v1 = s->tone_out2nd[3].v2;
655             s->tone_out2nd[5].v2 = s->tone_out2nd[6].v3;
656             s->tone_out2nd[5].v3 = s->tone_out2nd[6].fac*s->tone_out2nd[3].v2 - v1 + famp;
657
658         }
659 #endif
660         s->current_sample += (limit - sample);
661         if (s->current_sample < MF_GSIZE) {
662                         if (hit && !((digitmode & DSP_DIGITMODE_NOQUELCH))) {
663                                 /* If we had a hit last time, go ahead and clear this out since likely it
664                                    will be another hit */
665                                 for (i=sample;i<limit;i++) 
666                                         amp[i] = 0;
667                                 *writeback = 1;
668                         }
669             continue;
670                 }
671
672                 /* We're at the end of an MF detection block.  Go ahead and calculate
673                    all the energies. */
674                 for (i=0;i<6;i++) {
675                         tone_energy[i] = goertzel_result(&s->tone_out[i]);
676                 }
677                 
678                 /* Find highest */
679                 best1 = 0;
680                 max = tone_energy[0];
681                 for (i=1;i<6;i++) {
682                         if (tone_energy[i] > max) {
683                                 max = tone_energy[i];
684                                 best1 = i;
685                         }
686                 }
687
688                 /* Find 2nd highest */
689                 if (best1)
690                         max = tone_energy[0];
691                 else
692                         max = tone_energy[1];
693                 best2 = 0;
694                 for (i=0;i<6;i++) {
695                         if (i == best1) continue;
696                         if (tone_energy[i] > max) {
697                                 max = tone_energy[i];
698                                 best2 = i;
699                         }
700                 }
701                 
702         hit = 0;
703                 sofarsogood=1;
704                 /* Check for relative energies */
705                 for (i=0;i<6;i++) {
706                         if (i == best1) continue;
707                         if (i == best2) continue;
708                         if (tone_energy[best1] < tone_energy[i] * MF_RELATIVE_PEAK) {
709                                 sofarsogood = 0;
710                                 break;
711                         }
712                         if (tone_energy[best2] < tone_energy[i] * MF_RELATIVE_PEAK) {
713                                 sofarsogood = 0;
714                                 break;
715                         }
716                 }
717                 
718                 if (sofarsogood) {
719                         /* Check for 2nd harmonic */
720                         if (goertzel_result(&s->tone_out2nd[best1]) * MF_2ND_HARMONIC > tone_energy[best1]) 
721                                 sofarsogood = 0;
722                         else if (goertzel_result(&s->tone_out2nd[best2]) * MF_2ND_HARMONIC > tone_energy[best2])
723                                 sofarsogood = 0;
724                 }
725                 if (sofarsogood) {
726                         hit = mf_hit[best1][best2];
727                         if (!(digitmode & DSP_DIGITMODE_NOQUELCH)) {
728                                 /* Zero out frame data if this is part DTMF */
729                                 for (i=sample;i<limit;i++) 
730                                         amp[i] = 0;
731                                 *writeback = 1;
732                         }
733                         /* Look for two consecutive clean hits */
734                         if ((hit == s->hit3) && (s->hit3 != s->hit2)) {
735                                 s->mhit = hit;
736                                 s->detected_digits++;
737                                 if (s->current_digits < MAX_DTMF_DIGITS - 2) {
738                                         s->digits[s->current_digits++] = hit;
739                                         s->digits[s->current_digits] = '\0';
740                                 } else {
741                                         s->lost_digits++;
742                                 }
743                         }
744                 }
745                 
746         s->hit1 = s->hit2;
747         s->hit2 = s->hit3;
748         s->hit3 = hit;
749         /* Reinitialise the detector for the next block */
750         for (i = 0;  i < 6;  i++)
751         {
752             goertzel_reset(&s->tone_out[i]);
753             goertzel_reset(&s->tone_out2nd[i]);
754         }
755                 s->energy = 0.0;
756         s->current_sample = 0;
757     }
758     if ((!s->mhit) || (s->mhit != hit))
759     {
760                 s->mhit = 0;
761                 return(0);
762     }
763     return (hit);
764 }
765
766 static int __ast_dsp_digitdetect(struct ast_dsp *dsp, short *s, int len, int *writeback)
767 {
768         int res;
769         if (dsp->digitmode & DSP_DIGITMODE_MF)
770                 res = mf_detect(&dsp->td.mf, s, len, dsp->digitmode & DSP_DIGITMODE_RELAXDTMF, writeback);
771         else
772                 res = dtmf_detect(&dsp->td.dtmf, s, len, dsp->digitmode & DSP_DIGITMODE_RELAXDTMF, writeback);
773         return res;
774 }
775
776 int ast_dsp_digitdetect(struct ast_dsp *dsp, struct ast_frame *inf)
777 {
778         short *s;
779         int len;
780         int ign=0;
781         if (inf->frametype != AST_FRAME_VOICE) {
782                 ast_log(LOG_WARNING, "Can't check call progress of non-voice frames\n");
783                 return 0;
784         }
785         if (inf->subclass != AST_FORMAT_SLINEAR) {
786                 ast_log(LOG_WARNING, "Can only check call progress in signed-linear frames\n");
787                 return 0;
788         }
789         s = inf->data;
790         len = inf->datalen / 2;
791         return __ast_dsp_digitdetect(dsp, s, len, &ign);
792 }
793
794 static inline int pair_there(float p1, float p2, float i1, float i2, float e)
795 {
796         /* See if p1 and p2 are there, relative to i1 and i2 and total energy */
797         /* Make sure absolute levels are high enough */
798         if ((p1 < TONE_MIN_THRESH) || (p2 < TONE_MIN_THRESH))
799                 return 0;
800         /* Amplify ignored stuff */
801         i2 *= TONE_THRESH;
802         i1 *= TONE_THRESH;
803         e *= TONE_THRESH;
804         /* Check first tone */
805         if ((p1 < i1) || (p1 < i2) || (p1 < e))
806                 return 0;
807         /* And second */
808         if ((p2 < i1) || (p2 < i2) || (p2 < e))
809                 return 0;
810         /* Guess it's there... */
811         return 1;
812 }
813
814 int ast_dsp_getdigits (struct ast_dsp *dsp,
815               char *buf,
816               int max)
817 {
818         if (dsp->digitmode & DSP_DIGITMODE_MF) {
819             if (max > dsp->td.mf.current_digits)
820                 max = dsp->td.mf.current_digits;
821             if (max > 0)
822             {
823                 memcpy (buf, dsp->td.mf.digits, max);
824                 memmove (dsp->td.mf.digits, dsp->td.mf.digits + max, dsp->td.mf.current_digits - max);
825                 dsp->td.mf.current_digits -= max;
826             }
827             buf[max] = '\0';
828             return  max;
829         } else {
830             if (max > dsp->td.dtmf.current_digits)
831                 max = dsp->td.dtmf.current_digits;
832             if (max > 0)
833             {
834                 memcpy (buf, dsp->td.dtmf.digits, max);
835                 memmove (dsp->td.dtmf.digits, dsp->td.dtmf.digits + max, dsp->td.dtmf.current_digits - max);
836                 dsp->td.dtmf.current_digits -= max;
837             }
838             buf[max] = '\0';
839             return  max;
840         }
841 }
842
843 static int __ast_dsp_call_progress(struct ast_dsp *dsp, short *s, int len)
844 {
845         int x;
846         int pass;
847         int newstate = TONE_STATE_SILENCE;
848         int res = 0;
849         while(len) {
850                 /* Take the lesser of the number of samples we need and what we have */
851                 pass = len;
852                 if (pass > GSAMP_SIZE - dsp->gsamps) 
853                         pass = GSAMP_SIZE - dsp->gsamps;
854                 for (x=0;x<pass;x++) {
855                         goertzel_sample(&dsp->freqs[HZ_350], s[x]);
856                         goertzel_sample(&dsp->freqs[HZ_440], s[x]);
857                         goertzel_sample(&dsp->freqs[HZ_480], s[x]);
858                         goertzel_sample(&dsp->freqs[HZ_620], s[x]);
859                         goertzel_sample(&dsp->freqs[HZ_950], s[x]);
860                         goertzel_sample(&dsp->freqs[HZ_1400], s[x]);
861                         goertzel_sample(&dsp->freqs[HZ_1800], s[x]);
862                         dsp->genergy += s[x] * s[x];
863                 }
864                 s += pass;
865                 dsp->gsamps += pass;
866                 len -= pass;
867                 if (dsp->gsamps == GSAMP_SIZE) {
868                         float hz_350;
869                         float hz_440;
870                         float hz_480;
871                         float hz_620;
872                         float hz_950;
873                         float hz_1400;
874                         float hz_1800;
875                         hz_350 = goertzel_result(&dsp->freqs[HZ_350]);
876                         hz_440 = goertzel_result(&dsp->freqs[HZ_440]);
877                         hz_480 = goertzel_result(&dsp->freqs[HZ_480]);
878                         hz_620 = goertzel_result(&dsp->freqs[HZ_620]);
879                         hz_950 = goertzel_result(&dsp->freqs[HZ_950]);
880                         hz_1400 = goertzel_result(&dsp->freqs[HZ_1400]);
881                         hz_1800 = goertzel_result(&dsp->freqs[HZ_1800]);
882 #if 0
883                         printf("Got whole dsp state: 350: %e, 440: %e, 480: %e, 620: %e, 950: %e, 1400: %e, 1800: %e, Energy: %e\n", 
884                                 hz_350, hz_440, hz_480, hz_620, hz_950, hz_1400, hz_1800, dsp->genergy);
885 #endif
886                         if (pair_there(hz_480, hz_620, hz_350, hz_440, dsp->genergy)) {
887                                 newstate = TONE_STATE_BUSY;
888                         } else if (pair_there(hz_440, hz_480, hz_350, hz_620, dsp->genergy)) {
889                                 newstate = TONE_STATE_RINGING;
890                         } else if (pair_there(hz_350, hz_440, hz_480, hz_620, dsp->genergy)) {
891                                 newstate = TONE_STATE_DIALTONE;
892                         } else if (hz_950 > TONE_MIN_THRESH * TONE_THRESH) {
893                                 newstate = TONE_STATE_SPECIAL1;
894                         } else if (hz_1400 > TONE_MIN_THRESH * TONE_THRESH) {
895                                 if (dsp->tstate == TONE_STATE_SPECIAL1)
896                                         newstate = TONE_STATE_SPECIAL2;
897                         } else if (hz_1800 > TONE_MIN_THRESH * TONE_THRESH) {
898                                 if (dsp->tstate == TONE_STATE_SPECIAL2)
899                                         newstate = TONE_STATE_SPECIAL3;
900                         } else if (dsp->genergy > TONE_MIN_THRESH * TONE_THRESH) {
901                                 newstate = TONE_STATE_TALKING;
902                         } else
903                                 newstate = TONE_STATE_SILENCE;
904                         
905                         if (newstate == dsp->tstate) {
906                                 dsp->tcount++;
907                                 if (dsp->tcount == COUNT_THRESH) {
908                                         if (dsp->tstate == TONE_STATE_BUSY) {
909                                                 res = AST_CONTROL_BUSY;
910                                                 dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
911                                         } else if (dsp->tstate == TONE_STATE_TALKING) {
912                                                 res = AST_CONTROL_ANSWER;
913                                                 dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
914                                         } else if (dsp->tstate == TONE_STATE_RINGING)
915                                                 res = AST_CONTROL_RINGING;
916                                         else if (dsp->tstate == TONE_STATE_SPECIAL3) {
917                                                 res = AST_CONTROL_CONGESTION;
918                                                 dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
919                                         }
920                                         
921                                 }
922                         } else {
923 #if 0
924                                 printf("Newstate: %d\n", newstate);
925 #endif
926                                 dsp->tstate = newstate;
927                                 dsp->tcount = 1;
928                         }
929                         
930                         /* Reset goertzel */                                            
931                         for (x=0;x<7;x++)
932                                 dsp->freqs[x].v2 = dsp->freqs[x].v3 = 0.0;
933                         dsp->gsamps = 0;
934                         dsp->genergy = 0.0;
935                 }
936         }
937 #if 0
938         if (res)
939                 printf("Returning %d\n", res);
940 #endif          
941         return res;
942 }
943
944 int ast_dsp_call_progress(struct ast_dsp *dsp, struct ast_frame *inf)
945 {
946         if (inf->frametype != AST_FRAME_VOICE) {
947                 ast_log(LOG_WARNING, "Can't check call progress of non-voice frames\n");
948                 return 0;
949         }
950         if (inf->subclass != AST_FORMAT_SLINEAR) {
951                 ast_log(LOG_WARNING, "Can only check call progress in signed-linear frames\n");
952                 return 0;
953         }
954         return __ast_dsp_call_progress(dsp, inf->data, inf->datalen / 2);
955 }
956
957 static int __ast_dsp_silence(struct ast_dsp *dsp, short *s, int len, int *totalsilence)
958 {
959         int accum;
960         int x;
961         int res = 0;
962         
963         accum = 0;
964         for (x=0;x<len; x++) 
965                 accum += abs(s[x]);
966         accum /= x;
967         if (accum < dsp->threshold) {
968                 dsp->totalsilence += len/8;
969                 if (dsp->totalnoise) {
970                         /* Move and save history */
971                         memmove(dsp->historicnoise, dsp->historicnoise + 1, sizeof(dsp->historicnoise) - sizeof(dsp->historicnoise[0]));
972                         dsp->historicnoise[DSP_HISTORY - 1] = dsp->totalnoise;
973                         dsp->busymaybe = 1;
974                 }
975                 dsp->totalnoise = 0;
976                 res = 1;
977         } else {
978                 dsp->totalnoise += len/8;
979                 if (dsp->totalsilence) {
980                         /* Move and save history */
981                         memmove(dsp->historicsilence, dsp->historicsilence + 1, sizeof(dsp->historicsilence) - sizeof(dsp->historicsilence[0]));
982                         dsp->historicsilence[DSP_HISTORY - 1] = dsp->totalsilence;
983                         dsp->busymaybe = 1;
984                 }
985                 dsp->totalsilence = 0;
986         }
987         if (totalsilence)
988                 *totalsilence = dsp->totalsilence;
989         return res;
990 }
991
992 int ast_dsp_busydetect(struct ast_dsp *dsp)
993 {
994         int x;
995         int res = 0;
996         int max, min;
997         if (dsp->busymaybe) {
998 #if 0
999                 printf("Maybe busy!\n");
1000 #endif          
1001                 dsp->busymaybe = 0;
1002                 min = 9999;
1003                 max = 0;
1004                 for (x=DSP_HISTORY - dsp->busycount;x<DSP_HISTORY;x++) {
1005 #if 0
1006                         printf("Silence: %d, Noise: %d\n", dsp->historicsilence[x], dsp->historicnoise[x]);
1007 #endif                  
1008                         if (dsp->historicsilence[x] < min)
1009                                 min = dsp->historicsilence[x];
1010                         if (dsp->historicnoise[x] < min)
1011                                 min = dsp->historicnoise[x];
1012                         if (dsp->historicsilence[x] > max)
1013                                 max = dsp->historicsilence[x];
1014                         if (dsp->historicnoise[x] > max)
1015                                 max = dsp->historicnoise[x];
1016                 }
1017                 if ((max - min < BUSY_THRESHOLD) && (max < BUSY_MAX) && (min > BUSY_MIN)) {
1018 #if 0
1019                         printf("Busy!\n");
1020 #endif                  
1021                         res = 1;
1022                 }
1023 #if 0
1024                 printf("Min: %d, max: %d\n", min, max);
1025 #endif          
1026         }
1027         return res;
1028 }
1029
1030 int ast_dsp_silence(struct ast_dsp *dsp, struct ast_frame *f, int *totalsilence)
1031 {
1032         short *s;
1033         int len;
1034         
1035         if (f->frametype != AST_FRAME_VOICE) {
1036                 ast_log(LOG_WARNING, "Can't calculate silence on a non-voice frame\n");
1037                 return 0;
1038         }
1039         if (f->subclass != AST_FORMAT_SLINEAR) {
1040                 ast_log(LOG_WARNING, "Can only calculate silence on signed-linear frames :(\n");
1041                 return 0;
1042         }
1043         s = f->data;
1044         len = f->datalen/2;
1045         return __ast_dsp_silence(dsp, s, len, totalsilence);
1046 }
1047
1048 struct ast_frame *ast_dsp_process(struct ast_channel *chan, struct ast_dsp *dsp, struct ast_frame *af, int needlock)
1049 {
1050         int silence;
1051         int res;
1052         int digit;
1053         int x;
1054         unsigned short *shortdata;
1055         unsigned char *odata;
1056         int len;
1057         int writeback = 0;
1058
1059 #define FIX_INF(inf) do { \
1060                 if (writeback) { \
1061                         switch(inf->subclass) { \
1062                         case AST_FORMAT_SLINEAR: \
1063                                 break; \
1064                         case AST_FORMAT_ULAW: \
1065                                 for (x=0;x<len;x++) \
1066                                         odata[x] = AST_LIN2MU(shortdata[x]); \
1067                                 break; \
1068                         case AST_FORMAT_ALAW: \
1069                                 for (x=0;x<len;x++) \
1070                                         odata[x] = AST_LIN2A(shortdata[x]); \
1071                                 break; \
1072                         } \
1073                 } \
1074         } while(0) 
1075
1076         if (!af)
1077                 return NULL;
1078         if (af->frametype != AST_FRAME_VOICE)
1079                 return af;
1080         odata = af->data;
1081         len = af->datalen;
1082         /* Make sure we have short data */
1083         switch(af->subclass) {
1084         case AST_FORMAT_SLINEAR:
1085                 shortdata = af->data;
1086                 len = af->datalen / 2;
1087                 break;
1088         case AST_FORMAT_ULAW:
1089                 shortdata = alloca(af->datalen * 2);
1090                 if (!shortdata) {
1091                         ast_log(LOG_WARNING, "Unable to allocate stack space for data: %s\n", strerror(errno));
1092                         return af;
1093                 }
1094                 for (x=0;x<len;x++) 
1095                         shortdata[x] = AST_MULAW(odata[x]);
1096                 break;
1097         case AST_FORMAT_ALAW:
1098                 shortdata = alloca(af->datalen * 2);
1099                 if (!shortdata) {
1100                         ast_log(LOG_WARNING, "Unable to allocate stack space for data: %s\n", strerror(errno));
1101                         return af;
1102                 }
1103                 for (x=0;x<len;x++) 
1104                         shortdata[x] = AST_ALAW(odata[x]);
1105                 break;
1106         default:
1107                 ast_log(LOG_WARNING, "Unable to detect process %d frames\n", af->subclass);
1108                 return af;
1109         }
1110         silence = __ast_dsp_silence(dsp, shortdata, len, NULL);
1111         if ((dsp->features & DSP_FEATURE_SILENCE_SUPPRESS) && silence) {
1112                 memset(&dsp->f, 0, sizeof(dsp->f));
1113                 dsp->f.frametype = AST_FRAME_NULL;
1114                 return &dsp->f;
1115         }
1116         if ((dsp->features & DSP_FEATURE_BUSY_DETECT) && ast_dsp_busydetect(dsp)) {
1117                 memset(&dsp->f, 0, sizeof(dsp->f));
1118                 dsp->f.frametype = AST_FRAME_CONTROL;
1119                 dsp->f.subclass = AST_CONTROL_BUSY;
1120                 return &dsp->f;
1121         }
1122         if ((dsp->features & DSP_FEATURE_DTMF_DETECT)) {
1123                 digit = __ast_dsp_digitdetect(dsp, shortdata, len, &writeback);
1124 #if 0
1125                 if (digit)
1126                         printf("Performing digit detection returned %d, digitmode is %d\n", digit, dsp->digitmode);
1127 #endif                  
1128                 if (dsp->digitmode & (DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX)) {
1129                         if (!dsp->thinkdigit) {
1130                                 if (digit) {
1131                                         /* Looks like we might have something.  Request a conference mute for the moment */
1132                                         memset(&dsp->f, 0, sizeof(dsp->f));
1133                                         dsp->f.frametype = AST_FRAME_DTMF;
1134                                         dsp->f.subclass = 'm';
1135                                         dsp->thinkdigit = 'x';
1136                                         FIX_INF(af);
1137                                         if (chan)
1138                                                 ast_queue_frame(chan, af, needlock);
1139                                         ast_frfree(af);
1140                                         return &dsp->f;
1141                                 }
1142                         } else {
1143                                 if (digit) {
1144                                         /* Thought we saw one last time.  Pretty sure we really have now */
1145                                         if (dsp->thinkdigit) {
1146                                                 if ((dsp->thinkdigit != 'x') && (dsp->thinkdigit != digit)) {
1147                                                         /* If we found a digit, and we're changing digits, go
1148                                                            ahead and send this one, but DON'T stop confmute because
1149                                                            we're detecting something else, too... */
1150                                                         memset(&dsp->f, 0, sizeof(dsp->f));
1151                                                         dsp->f.frametype = AST_FRAME_DTMF;
1152                                                         dsp->f.subclass = dsp->thinkdigit;
1153                                                         FIX_INF(af);
1154                                                         if (chan)
1155                                                                 ast_queue_frame(chan, af, needlock);
1156                                                         ast_frfree(af);
1157                                                 }
1158                                                 dsp->thinkdigit = digit;
1159                                                 return &dsp->f;
1160                                         }
1161                                         dsp->thinkdigit = digit;
1162                                 } else {
1163                                         if (dsp->thinkdigit) {
1164                                                 memset(&dsp->f, 0, sizeof(dsp->f));
1165                                                 if (dsp->thinkdigit != 'x') {
1166                                                         /* If we found a digit, send it now */
1167                                                         dsp->f.frametype = AST_FRAME_DTMF;
1168                                                         dsp->f.subclass = dsp->thinkdigit;
1169                                                         if (chan)
1170                                                                 ast_queue_frame(chan, &dsp->f, needlock);
1171                                                 }
1172                                                 dsp->f.frametype = AST_FRAME_DTMF;
1173                                                 dsp->f.subclass = 'u';
1174                                                 dsp->thinkdigit = 0;
1175                                                 FIX_INF(af);
1176                                                 if (chan)
1177                                                         ast_queue_frame(chan, af, needlock);
1178                                                 ast_frfree(af);
1179                                                 return &dsp->f;
1180                                         }
1181                                 }
1182                         }
1183                 } else if (!digit) {
1184                         /* Only check when there is *not* a hit... */
1185                         if (dsp->digitmode & DSP_DIGITMODE_MF) {
1186                                 if (dsp->td.mf.current_digits) {
1187                                         memset(&dsp->f, 0, sizeof(dsp->f));
1188                                         dsp->f.frametype = AST_FRAME_DTMF;
1189                                         dsp->f.subclass = dsp->td.mf.digits[0];
1190                                         memmove(dsp->td.mf.digits, dsp->td.mf.digits + 1, dsp->td.mf.current_digits);
1191                                         dsp->td.mf.current_digits--;
1192                                         FIX_INF(af);
1193                                         if (chan)
1194                                                 ast_queue_frame(chan, af, needlock);
1195                                         ast_frfree(af);
1196                                         return &dsp->f;
1197                                 }
1198                         } else {
1199                                 if (dsp->td.dtmf.current_digits) {
1200                                         memset(&dsp->f, 0, sizeof(dsp->f));
1201                                         dsp->f.frametype = AST_FRAME_DTMF;
1202                                         dsp->f.subclass = dsp->td.dtmf.digits[0];
1203                                         memmove(dsp->td.dtmf.digits, dsp->td.dtmf.digits + 1, dsp->td.dtmf.current_digits);
1204                                         dsp->td.dtmf.current_digits--;
1205                                         FIX_INF(af);
1206                                         if (chan)
1207                                                 ast_queue_frame(chan, af, needlock);
1208                                         ast_frfree(af);
1209                                         return &dsp->f;
1210                                 }
1211                         }
1212                 }
1213         }
1214         if ((dsp->features & DSP_FEATURE_CALL_PROGRESS)) {
1215                 res = __ast_dsp_call_progress(dsp, shortdata, len);
1216                 memset(&dsp->f, 0, sizeof(dsp->f));
1217                 dsp->f.frametype = AST_FRAME_CONTROL;
1218                 if (res) {
1219                         switch(res) {
1220                         case AST_CONTROL_ANSWER:
1221                         case AST_CONTROL_BUSY:
1222                         case AST_CONTROL_RINGING:
1223                         case AST_CONTROL_CONGESTION:
1224                                 dsp->f.subclass = res;
1225                                 if (chan) 
1226                                         ast_queue_frame(chan, &dsp->f, needlock);
1227                                 break;
1228                         default:
1229                                 ast_log(LOG_WARNING, "Don't know how to represent call progress message %d\n", res);
1230                         }
1231                 }
1232         }
1233         FIX_INF(af);
1234         return af;
1235 }
1236
1237 struct ast_dsp *ast_dsp_new(void)
1238 {
1239         struct ast_dsp *dsp;
1240         dsp = malloc(sizeof(struct ast_dsp));
1241         if (dsp) {
1242                 memset(dsp, 0, sizeof(struct ast_dsp));
1243                 dsp->threshold = DEFAULT_THRESHOLD;
1244                 dsp->features = DSP_FEATURE_SILENCE_SUPPRESS;
1245                 dsp->busycount = 3;
1246                 /* Initialize goertzels */
1247                 goertzel_init(&dsp->freqs[HZ_350], 350.0);
1248                 goertzel_init(&dsp->freqs[HZ_440], 440.0);
1249                 goertzel_init(&dsp->freqs[HZ_480], 480.0);
1250                 goertzel_init(&dsp->freqs[HZ_620], 620.0);
1251                 goertzel_init(&dsp->freqs[HZ_950], 950.0);
1252                 goertzel_init(&dsp->freqs[HZ_1400], 1400.0);
1253                 goertzel_init(&dsp->freqs[HZ_1800], 1800.0);
1254                 /* Initialize DTMF detector */
1255                 ast_dtmf_detect_init(&dsp->td.dtmf);
1256         }
1257         return dsp;
1258 }
1259
1260 void ast_dsp_set_features(struct ast_dsp *dsp, int features)
1261 {
1262         dsp->features = features;
1263 }
1264
1265 void ast_dsp_free(struct ast_dsp *dsp)
1266 {
1267         free(dsp);
1268 }
1269
1270 void ast_dsp_set_threshold(struct ast_dsp *dsp, int threshold)
1271 {
1272         dsp->threshold = threshold;
1273 }
1274
1275 void ast_dsp_set_busy_count(struct ast_dsp *dsp, int cadences)
1276 {
1277         if (cadences < 1)
1278                 cadences = 1;
1279         if (cadences > DSP_HISTORY)
1280                 cadences = DSP_HISTORY;
1281         dsp->busycount = cadences;
1282 }
1283
1284 void ast_dsp_digitreset(struct ast_dsp *dsp)
1285 {
1286         int i;
1287         dsp->thinkdigit = 0;
1288         if (dsp->digitmode & DSP_DIGITMODE_MF) {
1289                 memset(dsp->td.mf.digits, 0, sizeof(dsp->td.mf.digits));
1290                 dsp->td.mf.current_digits = 0;
1291                 /* Reinitialise the detector for the next block */
1292                 for (i = 0;  i < 6;  i++) {
1293                 goertzel_reset(&dsp->td.mf.tone_out[i]);
1294                     goertzel_reset(&dsp->td.mf.tone_out2nd[i]);
1295                 }
1296                 dsp->td.mf.energy = 0.0;
1297                 dsp->td.mf.current_sample = 0;
1298             dsp->td.mf.hit1 = dsp->td.mf.hit2 = dsp->td.mf.hit3 = dsp->td.mf.hit4 = dsp->td.mf.mhit = 0;
1299         } else {
1300                 memset(dsp->td.dtmf.digits, 0, sizeof(dsp->td.dtmf.digits));
1301                 dsp->td.dtmf.current_digits = 0;
1302                 /* Reinitialise the detector for the next block */
1303                 for (i = 0;  i < 4;  i++) {
1304                 goertzel_reset(&dsp->td.dtmf.row_out[i]);
1305                     goertzel_reset(&dsp->td.dtmf.col_out[i]);
1306                 goertzel_reset(&dsp->td.dtmf.row_out2nd[i]);
1307                 goertzel_reset(&dsp->td.dtmf.col_out2nd[i]);
1308                 }
1309             goertzel_reset (&dsp->td.dtmf.fax_tone);
1310             goertzel_reset (&dsp->td.dtmf.fax_tone2nd);
1311                 dsp->td.dtmf.energy = 0.0;
1312                 dsp->td.dtmf.current_sample = 0;
1313             dsp->td.dtmf.hit1 = dsp->td.dtmf.hit2 = dsp->td.dtmf.hit3 = dsp->td.dtmf.hit4 = dsp->td.dtmf.mhit = 0;
1314         }
1315 }
1316
1317 void ast_dsp_reset(struct ast_dsp *dsp)
1318 {
1319         int x;
1320         dsp->totalsilence = 0;
1321         dsp->gsamps = 0;
1322         for (x=0;x<4;x++)
1323                 dsp->freqs[x].v2 = dsp->freqs[x].v3 = 0.0;
1324         memset(dsp->historicsilence, 0, sizeof(dsp->historicsilence));
1325         memset(dsp->historicnoise, 0, sizeof(dsp->historicnoise));
1326         
1327 }
1328
1329 int ast_dsp_digitmode(struct ast_dsp *dsp, int digitmode)
1330 {
1331         int new, old;
1332         old = dsp->digitmode & (DSP_DIGITMODE_DTMF | DSP_DIGITMODE_MF | DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX);
1333         new = digitmode & (DSP_DIGITMODE_DTMF | DSP_DIGITMODE_MF | DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX);
1334         if (old != new) {
1335                 /* Must initialize structures if switching from MF to DTMF or vice-versa */
1336                 if (new & DSP_DIGITMODE_MF)
1337                         ast_mf_detect_init(&dsp->td.mf);
1338                 else
1339                         ast_dtmf_detect_init(&dsp->td.dtmf);
1340         }
1341         dsp->digitmode = digitmode;
1342         return 0;
1343 }