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