Sun Mar 9 07:00:01 CET 2003
[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                                  hit = 'f';
523                                  s->mhit = 'f';
524                      s->detected_digits++;
525                      if (s->current_digits < MAX_DTMF_DIGITS)
526                      {
527                           s->digits[s->current_digits++] = hit;
528                           s->digits[s->current_digits] = '\0';
529                      }
530                      else
531                      {
532                            s->lost_digits++;
533                      }
534                         }
535                         s->fax_hits = 0;
536                 }
537         s->hit1 = s->hit2;
538         s->hit2 = s->hit3;
539         s->hit3 = hit;
540         /* Reinitialise the detector for the next block */
541         for (i = 0;  i < 4;  i++)
542         {
543             goertzel_reset(&s->row_out[i]);
544             goertzel_reset(&s->col_out[i]);
545             goertzel_reset(&s->row_out2nd[i]);
546             goertzel_reset(&s->col_out2nd[i]);
547         }
548         goertzel_reset (&s->fax_tone);
549         goertzel_reset (&s->fax_tone2nd);
550                 s->energy = 0.0;
551         s->current_sample = 0;
552     }
553     if ((!s->mhit) || (s->mhit != hit))
554     {
555         s->mhit = 0;
556         return(0);
557     }
558     return (hit);
559 }
560
561 /* MF goertzel size */
562 #define MF_GSIZE 160
563
564 static int mf_detect (mf_detect_state_t *s,
565                  int16_t amp[],
566                  int samples, 
567                  int digitmode, int *writeback)
568 {
569
570     float tone_energy[6];
571     float famp;
572     float v1;
573     int i;
574     int j;
575     int sample;
576     int best1;
577     int best2;
578         float max;
579     int hit;
580     int limit;
581         int sofarsogood;
582
583     hit = 0;
584     for (sample = 0;  sample < samples;  sample = limit)
585     {
586         /* 80 is optimised to meet the MF specs. */
587         if ((samples - sample) >= (MF_GSIZE - s->current_sample))
588             limit = sample + (MF_GSIZE - s->current_sample);
589         else
590             limit = samples;
591 #if defined(USE_3DNOW)
592         _dtmf_goertzel_update (s->row_out, amp + sample, limit - sample);
593         _dtmf_goertzel_update (s->col_out, amp + sample, limit - sample);
594         _dtmf_goertzel_update (s->row_out2nd, amp + sample, limit2 - sample);
595         _dtmf_goertzel_update (s->col_out2nd, amp + sample, limit2 - sample);
596                 /* XXX Need to fax detect for 3dnow too XXX */
597                 #warning "Fax Support Broken"
598 #else
599         /* The following unrolled loop takes only 35% (rough estimate) of the 
600            time of a rolled loop on the machine on which it was developed */
601         for (j = sample;  j < limit;  j++)
602         {
603             famp = amp[j];
604             
605             s->energy += famp*famp;
606             
607             /* With GCC 2.95, the following unrolled code seems to take about 35%
608                (rough estimate) as long as a neat little 0-3 loop */
609             v1 = s->tone_out[0].v2;
610             s->tone_out[0].v2 = s->tone_out[0].v3;
611             s->tone_out[0].v3 = s->tone_out[0].fac*s->tone_out[0].v2 - v1 + famp;
612
613             v1 = s->tone_out[1].v2;
614             s->tone_out[1].v2 = s->tone_out[1].v3;
615             s->tone_out[1].v3 = s->tone_out[1].fac*s->tone_out[1].v2 - v1 + famp;
616     
617             v1 = s->tone_out[2].v2;
618             s->tone_out[2].v2 = s->tone_out[2].v3;
619             s->tone_out[2].v3 = s->tone_out[2].fac*s->tone_out[2].v2 - v1 + famp;
620     
621             v1 = s->tone_out[3].v2;
622             s->tone_out[3].v2 = s->tone_out[3].v3;
623             s->tone_out[3].v3 = s->tone_out[3].fac*s->tone_out[3].v2 - v1 + famp;
624
625             v1 = s->tone_out[4].v2;
626             s->tone_out[4].v2 = s->tone_out[4].v3;
627             s->tone_out[4].v3 = s->tone_out[4].fac*s->tone_out[4].v2 - v1 + famp;
628
629             v1 = s->tone_out[5].v2;
630             s->tone_out[5].v2 = s->tone_out[5].v3;
631             s->tone_out[5].v3 = s->tone_out[5].fac*s->tone_out[5].v2 - v1 + famp;
632
633             v1 = s->tone_out2nd[0].v2;
634             s->tone_out2nd[0].v2 = s->tone_out2nd[0].v3;
635             s->tone_out2nd[0].v3 = s->tone_out2nd[0].fac*s->tone_out2nd[0].v2 - v1 + famp;
636         
637             v1 = s->tone_out2nd[1].v2;
638             s->tone_out2nd[1].v2 = s->tone_out2nd[1].v3;
639             s->tone_out2nd[1].v3 = s->tone_out2nd[1].fac*s->tone_out2nd[1].v2 - v1 + famp;
640         
641             v1 = s->tone_out2nd[2].v2;
642             s->tone_out2nd[2].v2 = s->tone_out2nd[2].v3;
643             s->tone_out2nd[2].v3 = s->tone_out2nd[2].fac*s->tone_out2nd[2].v2 - v1 + famp;
644         
645             v1 = s->tone_out2nd[3].v2;
646             s->tone_out2nd[3].v2 = s->tone_out2nd[3].v3;
647             s->tone_out2nd[3].v3 = s->tone_out2nd[3].fac*s->tone_out2nd[3].v2 - v1 + famp;
648
649             v1 = s->tone_out2nd[4].v2;
650             s->tone_out2nd[4].v2 = s->tone_out2nd[4].v3;
651             s->tone_out2nd[4].v3 = s->tone_out2nd[4].fac*s->tone_out2nd[2].v2 - v1 + famp;
652         
653             v1 = s->tone_out2nd[3].v2;
654             s->tone_out2nd[5].v2 = s->tone_out2nd[6].v3;
655             s->tone_out2nd[5].v3 = s->tone_out2nd[6].fac*s->tone_out2nd[3].v2 - v1 + famp;
656
657         }
658 #endif
659         s->current_sample += (limit - sample);
660         if (s->current_sample < MF_GSIZE) {
661                         if (hit && !((digitmode & DSP_DIGITMODE_NOQUELCH))) {
662                                 /* If we had a hit last time, go ahead and clear this out since likely it
663                                    will be another hit */
664                                 for (i=sample;i<limit;i++) 
665                                         amp[i] = 0;
666                                 *writeback = 1;
667                         }
668             continue;
669                 }
670
671                 /* We're at the end of an MF detection block.  Go ahead and calculate
672                    all the energies. */
673                 for (i=0;i<6;i++) {
674                         tone_energy[i] = goertzel_result(&s->tone_out[i]);
675                 }
676                 
677                 /* Find highest */
678                 best1 = 0;
679                 max = tone_energy[0];
680                 for (i=1;i<6;i++) {
681                         if (tone_energy[i] > max) {
682                                 max = tone_energy[i];
683                                 best1 = i;
684                         }
685                 }
686
687                 /* Find 2nd highest */
688                 if (best1)
689                         max = tone_energy[0];
690                 else
691                         max = tone_energy[1];
692                 best2 = 0;
693                 for (i=0;i<6;i++) {
694                         if (i == best1) continue;
695                         if (tone_energy[i] > max) {
696                                 max = tone_energy[i];
697                                 best2 = i;
698                         }
699                 }
700                 
701         hit = 0;
702                 sofarsogood=1;
703                 /* Check for relative energies */
704                 for (i=0;i<6;i++) {
705                         if (i == best1) continue;
706                         if (i == best2) continue;
707                         if (tone_energy[best1] < tone_energy[i] * MF_RELATIVE_PEAK) {
708                                 sofarsogood = 0;
709                                 break;
710                         }
711                         if (tone_energy[best2] < tone_energy[i] * MF_RELATIVE_PEAK) {
712                                 sofarsogood = 0;
713                                 break;
714                         }
715                 }
716                 
717                 if (sofarsogood) {
718                         /* Check for 2nd harmonic */
719                         if (goertzel_result(&s->tone_out2nd[best1]) * MF_2ND_HARMONIC > tone_energy[best1]) 
720                                 sofarsogood = 0;
721                         else if (goertzel_result(&s->tone_out2nd[best1]) * MF_2ND_HARMONIC > tone_energy[best2])
722                                 sofarsogood = 0;
723                 }
724                 if (sofarsogood) {
725                         hit = mf_hit[best1][best2];
726                         if (!(digitmode & DSP_DIGITMODE_NOQUELCH)) {
727                                 /* Zero out frame data if this is part DTMF */
728                                 for (i=sample;i<limit;i++) 
729                                         amp[i] = 0;
730                                 *writeback = 1;
731                         }
732                         /* Look for two consecutive clean hits */
733                         if ((hit == s->hit3) && (s->hit3 != s->hit2)) {
734                                 s->mhit = hit;
735                                 s->detected_digits++;
736                                 if (s->current_digits < MAX_DTMF_DIGITS - 2) {
737                                         s->digits[s->current_digits++] = hit;
738                                         s->digits[s->current_digits] = '\0';
739                                 } else {
740                                         s->lost_digits++;
741                                 }
742                         }
743                 }
744                 
745         s->hit1 = s->hit2;
746         s->hit2 = s->hit3;
747         s->hit3 = hit;
748         /* Reinitialise the detector for the next block */
749         for (i = 0;  i < 6;  i++)
750         {
751             goertzel_reset(&s->tone_out[i]);
752             goertzel_reset(&s->tone_out2nd[i]);
753         }
754                 s->energy = 0.0;
755         s->current_sample = 0;
756     }
757     if ((!s->mhit) || (s->mhit != hit))
758     {
759                 s->mhit = 0;
760                 return(0);
761     }
762     return (hit);
763 }
764
765 static int __ast_dsp_digitdetect(struct ast_dsp *dsp, short *s, int len, int *writeback)
766 {
767         int res;
768         if (dsp->digitmode & DSP_DIGITMODE_MF)
769                 res = mf_detect(&dsp->td.mf, s, len, dsp->digitmode & DSP_DIGITMODE_RELAXDTMF, writeback);
770         else
771                 res = dtmf_detect(&dsp->td.dtmf, s, len, dsp->digitmode & DSP_DIGITMODE_RELAXDTMF, writeback);
772         return res;
773 }
774
775 int ast_dsp_digitdetect(struct ast_dsp *dsp, struct ast_frame *inf)
776 {
777         short *s;
778         int len;
779         int ign=0;
780         if (inf->frametype != AST_FRAME_VOICE) {
781                 ast_log(LOG_WARNING, "Can't check call progress of non-voice frames\n");
782                 return 0;
783         }
784         if (inf->subclass != AST_FORMAT_SLINEAR) {
785                 ast_log(LOG_WARNING, "Can only check call progress in signed-linear frames\n");
786                 return 0;
787         }
788         s = inf->data;
789         len = inf->datalen / 2;
790         return __ast_dsp_digitdetect(dsp, s, len, &ign);
791 }
792
793 static inline int pair_there(float p1, float p2, float i1, float i2, float e)
794 {
795         /* See if p1 and p2 are there, relative to i1 and i2 and total energy */
796         /* Make sure absolute levels are high enough */
797         if ((p1 < TONE_MIN_THRESH) || (p2 < TONE_MIN_THRESH))
798                 return 0;
799         /* Amplify ignored stuff */
800         i2 *= TONE_THRESH;
801         i1 *= TONE_THRESH;
802         e *= TONE_THRESH;
803         /* Check first tone */
804         if ((p1 < i1) || (p1 < i2) || (p1 < e))
805                 return 0;
806         /* And second */
807         if ((p2 < i1) || (p2 < i2) || (p2 < e))
808                 return 0;
809         /* Guess it's there... */
810         return 1;
811 }
812
813 int ast_dsp_getdigits (struct ast_dsp *dsp,
814               char *buf,
815               int max)
816 {
817         if (dsp->digitmode & DSP_DIGITMODE_MF) {
818             if (max > dsp->td.mf.current_digits)
819                 max = dsp->td.mf.current_digits;
820             if (max > 0)
821             {
822                 memcpy (buf, dsp->td.mf.digits, max);
823                 memmove (dsp->td.mf.digits, dsp->td.mf.digits + max, dsp->td.mf.current_digits - max);
824                 dsp->td.mf.current_digits -= max;
825             }
826             buf[max] = '\0';
827             return  max;
828         } else {
829             if (max > dsp->td.dtmf.current_digits)
830                 max = dsp->td.dtmf.current_digits;
831             if (max > 0)
832             {
833                 memcpy (buf, dsp->td.dtmf.digits, max);
834                 memmove (dsp->td.dtmf.digits, dsp->td.dtmf.digits + max, dsp->td.dtmf.current_digits - max);
835                 dsp->td.dtmf.current_digits -= max;
836             }
837             buf[max] = '\0';
838             return  max;
839         }
840 }
841
842 static int __ast_dsp_call_progress(struct ast_dsp *dsp, short *s, int len)
843 {
844         int x;
845         int pass;
846         int newstate;
847         int res = 0;
848         while(len) {
849                 /* Take the lesser of the number of samples we need and what we have */
850                 pass = len;
851                 if (pass > GSAMP_SIZE - dsp->gsamps) 
852                         pass = GSAMP_SIZE - dsp->gsamps;
853                 for (x=0;x<pass;x++) {
854                         goertzel_sample(&dsp->freqs[HZ_350], s[x]);
855                         goertzel_sample(&dsp->freqs[HZ_440], s[x]);
856                         goertzel_sample(&dsp->freqs[HZ_480], s[x]);
857                         goertzel_sample(&dsp->freqs[HZ_620], s[x]);
858                         goertzel_sample(&dsp->freqs[HZ_950], s[x]);
859                         goertzel_sample(&dsp->freqs[HZ_1400], s[x]);
860                         goertzel_sample(&dsp->freqs[HZ_1800], s[x]);
861                         dsp->genergy += s[x] * s[x];
862                 }
863                 s += pass;
864                 dsp->gsamps += pass;
865                 len -= pass;
866                 if (dsp->gsamps == GSAMP_SIZE) {
867                         float hz_350;
868                         float hz_440;
869                         float hz_480;
870                         float hz_620;
871                         float hz_950;
872                         float hz_1400;
873                         float hz_1800;
874                         hz_350 = goertzel_result(&dsp->freqs[HZ_350]);
875                         hz_440 = goertzel_result(&dsp->freqs[HZ_440]);
876                         hz_480 = goertzel_result(&dsp->freqs[HZ_480]);
877                         hz_620 = goertzel_result(&dsp->freqs[HZ_620]);
878                         hz_950 = goertzel_result(&dsp->freqs[HZ_950]);
879                         hz_1400 = goertzel_result(&dsp->freqs[HZ_1400]);
880                         hz_1800 = goertzel_result(&dsp->freqs[HZ_1800]);
881 #if 0
882                         printf("Got whole dsp state: 350: %e, 440: %e, 480: %e, 620: %e, 950: %e, 1400: %e, 1800: %e, Energy: %e\n", 
883                                 hz_350, hz_440, hz_480, hz_620, hz_950, hz_1400, hz_1800, dsp->genergy);
884 #endif
885                         if (pair_there(hz_480, hz_620, hz_350, hz_440, dsp->genergy)) {
886                                 newstate = TONE_STATE_BUSY;
887                         } else if (pair_there(hz_440, hz_480, hz_350, hz_620, dsp->genergy)) {
888                                 newstate = TONE_STATE_RINGING;
889                         } else if (pair_there(hz_350, hz_440, hz_480, hz_620, dsp->genergy)) {
890                                 newstate = TONE_STATE_DIALTONE;
891                         } else if (hz_950 > TONE_MIN_THRESH * TONE_THRESH) {
892                                 newstate = TONE_STATE_SPECIAL1;
893                         } else if (hz_1400 > TONE_MIN_THRESH * TONE_THRESH) {
894                                 if (dsp->tstate == TONE_STATE_SPECIAL1)
895                                         newstate = TONE_STATE_SPECIAL2;
896                         } else if (hz_1800 > TONE_MIN_THRESH * TONE_THRESH) {
897                                 if (dsp->tstate == TONE_STATE_SPECIAL2)
898                                         newstate = TONE_STATE_SPECIAL3;
899                         } else if (dsp->genergy > TONE_MIN_THRESH * TONE_THRESH) {
900                                 newstate = TONE_STATE_TALKING;
901                         } else
902                                 newstate = TONE_STATE_SILENCE;
903                         
904                         if (newstate == dsp->tstate) {
905                                 dsp->tcount++;
906                                 if (dsp->tcount == COUNT_THRESH) {
907                                         if (dsp->tstate == TONE_STATE_BUSY) {
908                                                 res = AST_CONTROL_BUSY;
909                                                 dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
910                                         } else if (dsp->tstate == TONE_STATE_TALKING) {
911                                                 res = AST_CONTROL_ANSWER;
912                                                 dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
913                                         } else if (dsp->tstate == TONE_STATE_RINGING)
914                                                 res = AST_CONTROL_RINGING;
915                                         else if (dsp->tstate == TONE_STATE_SPECIAL3) {
916                                                 res = AST_CONTROL_CONGESTION;
917                                                 dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
918                                         }
919                                         
920                                 }
921                         } else {
922 #if 0
923                                 printf("Newstate: %d\n", newstate);
924 #endif
925                                 dsp->tstate = newstate;
926                                 dsp->tcount = 1;
927                         }
928                         
929                         /* Reset goertzel */                                            
930                         for (x=0;x<7;x++)
931                                 dsp->freqs[x].v2 = dsp->freqs[x].v3 = 0.0;
932                         dsp->gsamps = 0;
933                         dsp->genergy = 0.0;
934                 }
935         }
936 #if 0
937         if (res)
938                 printf("Returning %d\n", res);
939 #endif          
940         return res;
941 }
942
943 int ast_dsp_call_progress(struct ast_dsp *dsp, struct ast_frame *inf)
944 {
945         if (inf->frametype != AST_FRAME_VOICE) {
946                 ast_log(LOG_WARNING, "Can't check call progress of non-voice frames\n");
947                 return 0;
948         }
949         if (inf->subclass != AST_FORMAT_SLINEAR) {
950                 ast_log(LOG_WARNING, "Can only check call progress in signed-linear frames\n");
951                 return 0;
952         }
953         return __ast_dsp_call_progress(dsp, inf->data, inf->datalen / 2);
954 }
955
956 static int __ast_dsp_silence(struct ast_dsp *dsp, short *s, int len, int *totalsilence)
957 {
958         int accum;
959         int x;
960         int res = 0;
961         
962         accum = 0;
963         for (x=0;x<len; x++) 
964                 accum += abs(s[x]);
965         accum /= x;
966         if (accum < dsp->threshold) {
967                 dsp->totalsilence += len/8;
968                 if (dsp->totalnoise) {
969                         /* Move and save history */
970                         memmove(dsp->historicnoise, dsp->historicnoise + 1, sizeof(dsp->historicnoise) - sizeof(dsp->historicnoise[0]));
971                         dsp->historicnoise[DSP_HISTORY - 1] = dsp->totalnoise;
972                         dsp->busymaybe = 1;
973                 }
974                 dsp->totalnoise = 0;
975                 res = 1;
976         } else {
977                 dsp->totalnoise += len/8;
978                 if (dsp->totalsilence) {
979                         /* Move and save history */
980                         memmove(dsp->historicsilence, dsp->historicsilence + 1, sizeof(dsp->historicsilence) - sizeof(dsp->historicsilence[0]));
981                         dsp->historicsilence[DSP_HISTORY - 1] = dsp->totalsilence;
982                         dsp->busymaybe = 1;
983                 }
984                 dsp->totalsilence = 0;
985         }
986         if (totalsilence)
987                 *totalsilence = dsp->totalsilence;
988         return res;
989 }
990
991 int ast_dsp_busydetect(struct ast_dsp *dsp)
992 {
993         int x;
994         int res = 0;
995         int max, min;
996         if (dsp->busymaybe) {
997 #if 0
998                 printf("Maybe busy!\n");
999 #endif          
1000                 dsp->busymaybe = 0;
1001                 min = 9999;
1002                 max = 0;
1003                 for (x=DSP_HISTORY - dsp->busycount;x<DSP_HISTORY;x++) {
1004 #if 0
1005                         printf("Silence: %d, Noise: %d\n", dsp->historicsilence[x], dsp->historicnoise[x]);
1006 #endif                  
1007                         if (dsp->historicsilence[x] < min)
1008                                 min = dsp->historicsilence[x];
1009                         if (dsp->historicnoise[x] < min)
1010                                 min = dsp->historicnoise[x];
1011                         if (dsp->historicsilence[x] > max)
1012                                 max = dsp->historicsilence[x];
1013                         if (dsp->historicnoise[x] > max)
1014                                 max = dsp->historicnoise[x];
1015                 }
1016                 if ((max - min < BUSY_THRESHOLD) && (max < BUSY_MAX) && (min > BUSY_MIN)) {
1017 #if 0
1018                         printf("Busy!\n");
1019 #endif                  
1020                         res = 1;
1021                 }
1022 #if 0
1023                 printf("Min: %d, max: %d\n", min, max);
1024 #endif          
1025         }
1026         return res;
1027 }
1028
1029 int ast_dsp_silence(struct ast_dsp *dsp, struct ast_frame *f, int *totalsilence)
1030 {
1031         short *s;
1032         int len;
1033         
1034         if (f->frametype != AST_FRAME_VOICE) {
1035                 ast_log(LOG_WARNING, "Can't calculate silence on a non-voice frame\n");
1036                 return 0;
1037         }
1038         if (f->subclass != AST_FORMAT_SLINEAR) {
1039                 ast_log(LOG_WARNING, "Can only calculate silence on signed-linear frames :(\n");
1040                 return 0;
1041         }
1042         s = f->data;
1043         len = f->datalen/2;
1044         return __ast_dsp_silence(dsp, s, len, totalsilence);
1045 }
1046
1047 struct ast_frame *ast_dsp_process(struct ast_channel *chan, struct ast_dsp *dsp, struct ast_frame *af, int needlock)
1048 {
1049         int silence;
1050         int res;
1051         int digit;
1052         int x;
1053         unsigned short *shortdata;
1054         unsigned char *odata;
1055         int len;
1056         int writeback = 0;
1057
1058 #define FIX_INF(inf) do { \
1059                 if (writeback) { \
1060                         switch(inf->subclass) { \
1061                         case AST_FORMAT_SLINEAR: \
1062                                 break; \
1063                         case AST_FORMAT_ULAW: \
1064                                 for (x=0;x<len;x++) \
1065                                         odata[x] = AST_LIN2MU(shortdata[x]); \
1066                                 break; \
1067                         case AST_FORMAT_ALAW: \
1068                                 for (x=0;x<len;x++) \
1069                                         odata[x] = AST_LIN2A(shortdata[x]); \
1070                                 break; \
1071                         } \
1072                 } \
1073         } while(0) 
1074
1075         if (!af)
1076                 return NULL;
1077         if (af->frametype != AST_FRAME_VOICE)
1078                 return af;
1079         odata = af->data;
1080         len = af->datalen;
1081         /* Make sure we have short data */
1082         switch(af->subclass) {
1083         case AST_FORMAT_SLINEAR:
1084                 shortdata = af->data;
1085                 len = af->datalen / 2;
1086                 break;
1087         case AST_FORMAT_ULAW:
1088                 shortdata = alloca(af->datalen * 2);
1089                 if (!shortdata) {
1090                         ast_log(LOG_WARNING, "Unable to allocate stack space for data: %s\n", strerror(errno));
1091                         return af;
1092                 }
1093                 for (x=0;x<len;x++) 
1094                         shortdata[x] = AST_MULAW(odata[x]);
1095                 break;
1096         case AST_FORMAT_ALAW:
1097                 shortdata = alloca(af->datalen * 2);
1098                 if (!shortdata) {
1099                         ast_log(LOG_WARNING, "Unable to allocate stack space for data: %s\n", strerror(errno));
1100                         return af;
1101                 }
1102                 for (x=0;x<len;x++) 
1103                         shortdata[x] = AST_ALAW(odata[x]);
1104                 break;
1105         default:
1106                 ast_log(LOG_WARNING, "Unable to detect process %d frames\n", af->subclass);
1107                 return af;
1108         }
1109         silence = __ast_dsp_silence(dsp, shortdata, len, NULL);
1110         if ((dsp->features & DSP_FEATURE_SILENCE_SUPPRESS) && silence) {
1111                 memset(&dsp->f, 0, sizeof(dsp->f));
1112                 dsp->f.frametype = AST_FRAME_NULL;
1113                 return &dsp->f;
1114         }
1115         if ((dsp->features & DSP_FEATURE_BUSY_DETECT) && ast_dsp_busydetect(dsp)) {
1116                 memset(&dsp->f, 0, sizeof(dsp->f));
1117                 dsp->f.frametype = AST_FRAME_CONTROL;
1118                 dsp->f.subclass = AST_CONTROL_BUSY;
1119                 return &dsp->f;
1120         }
1121         if ((dsp->features & DSP_FEATURE_DTMF_DETECT)) {
1122                 digit = __ast_dsp_digitdetect(dsp, shortdata, len, &writeback);
1123 #if 0
1124                 if (digit)
1125                         printf("Performing digit detection returned %d, digitmode is %d\n", digit, dsp->digitmode);
1126 #endif                  
1127                 if (dsp->digitmode & (DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX)) {
1128                         if (!dsp->thinkdigit) {
1129                                 if (digit) {
1130                                         /* Looks like we might have something.  Request a conference mute for the moment */
1131                                         memset(&dsp->f, 0, sizeof(dsp->f));
1132                                         dsp->f.frametype = AST_FRAME_DTMF;
1133                                         dsp->f.subclass = 'm';
1134                                         dsp->thinkdigit = 'x';
1135                                         FIX_INF(af);
1136                                         if (chan)
1137                                                 ast_queue_frame(chan, af, needlock);
1138                                         ast_frfree(af);
1139                                         return &dsp->f;
1140                                 }
1141                         } else {
1142                                 if (digit) {
1143                                         /* Thought we saw one last time.  Pretty sure we really have now */
1144                                         if (dsp->thinkdigit) 
1145                                                 dsp->thinkdigit = digit;
1146                                 } else {
1147                                         if (dsp->thinkdigit) {
1148                                                 memset(&dsp->f, 0, sizeof(dsp->f));
1149                                                 if (dsp->thinkdigit != 'x') {
1150                                                         /* If we found a digit, send it now */
1151                                                         dsp->f.frametype = AST_FRAME_DTMF;
1152                                                         dsp->f.subclass = dsp->thinkdigit;
1153                                                         if (chan)
1154                                                                 ast_queue_frame(chan, &dsp->f, needlock);
1155                                                 }
1156                                                 dsp->f.frametype = AST_FRAME_DTMF;
1157                                                 dsp->f.subclass = 'u';
1158                                                 dsp->thinkdigit = 0;
1159                                                 FIX_INF(af);
1160                                                 if (chan)
1161                                                         ast_queue_frame(chan, af, needlock);
1162                                                 ast_frfree(af);
1163                                                 return &dsp->f;
1164                                         }
1165                                 }
1166                         }
1167                 } else if (!digit) {
1168                         /* Only check when there is *not* a hit... */
1169                         if (dsp->digitmode & DSP_DIGITMODE_MF) {
1170                                 if (dsp->td.mf.current_digits) {
1171                                         memset(&dsp->f, 0, sizeof(dsp->f));
1172                                         dsp->f.frametype = AST_FRAME_DTMF;
1173                                         dsp->f.subclass = dsp->td.mf.digits[0];
1174                                         memmove(dsp->td.mf.digits, dsp->td.mf.digits + 1, dsp->td.mf.current_digits);
1175                                         dsp->td.mf.current_digits--;
1176                                         FIX_INF(af);
1177                                         if (chan)
1178                                                 ast_queue_frame(chan, af, needlock);
1179                                         ast_frfree(af);
1180                                         return &dsp->f;
1181                                 }
1182                         } else {
1183                                 if (dsp->td.dtmf.current_digits) {
1184                                         memset(&dsp->f, 0, sizeof(dsp->f));
1185                                         dsp->f.frametype = AST_FRAME_DTMF;
1186                                         dsp->f.subclass = dsp->td.dtmf.digits[0];
1187                                         memmove(dsp->td.dtmf.digits, dsp->td.dtmf.digits + 1, dsp->td.dtmf.current_digits);
1188                                         dsp->td.dtmf.current_digits--;
1189                                         FIX_INF(af);
1190                                         if (chan)
1191                                                 ast_queue_frame(chan, af, needlock);
1192                                         ast_frfree(af);
1193                                         return &dsp->f;
1194                                 }
1195                         }
1196                 }
1197         }
1198         if ((dsp->features & DSP_FEATURE_CALL_PROGRESS)) {
1199                 res = __ast_dsp_call_progress(dsp, shortdata, len);
1200                 memset(&dsp->f, 0, sizeof(dsp->f));
1201                 dsp->f.frametype = AST_FRAME_CONTROL;
1202                 if (res) {
1203                         switch(res) {
1204                         case AST_CONTROL_ANSWER:
1205                         case AST_CONTROL_BUSY:
1206                         case AST_CONTROL_RINGING:
1207                         case AST_CONTROL_CONGESTION:
1208                                 dsp->f.subclass = res;
1209                                 if (chan) 
1210                                         ast_queue_frame(chan, &dsp->f, needlock);
1211                                 break;
1212                         default:
1213                                 ast_log(LOG_WARNING, "Don't know how to represent call progress message %d\n", res);
1214                         }
1215                 }
1216         }
1217         FIX_INF(af);
1218         return af;
1219 }
1220
1221 struct ast_dsp *ast_dsp_new(void)
1222 {
1223         struct ast_dsp *dsp;
1224         dsp = malloc(sizeof(struct ast_dsp));
1225         if (dsp) {
1226                 memset(dsp, 0, sizeof(struct ast_dsp));
1227                 dsp->threshold = DEFAULT_THRESHOLD;
1228                 dsp->features = DSP_FEATURE_SILENCE_SUPPRESS;
1229                 dsp->busycount = 3;
1230                 /* Initialize goertzels */
1231                 goertzel_init(&dsp->freqs[HZ_350], 350.0);
1232                 goertzel_init(&dsp->freqs[HZ_440], 440.0);
1233                 goertzel_init(&dsp->freqs[HZ_480], 480.0);
1234                 goertzel_init(&dsp->freqs[HZ_620], 620.0);
1235                 goertzel_init(&dsp->freqs[HZ_950], 950.0);
1236                 goertzel_init(&dsp->freqs[HZ_1400], 1400.0);
1237                 goertzel_init(&dsp->freqs[HZ_1800], 1800.0);
1238                 /* Initialize DTMF detector */
1239                 ast_dtmf_detect_init(&dsp->td.dtmf);
1240         }
1241         return dsp;
1242 }
1243
1244 void ast_dsp_set_features(struct ast_dsp *dsp, int features)
1245 {
1246         dsp->features = features;
1247 }
1248
1249 void ast_dsp_free(struct ast_dsp *dsp)
1250 {
1251         free(dsp);
1252 }
1253
1254 void ast_dsp_set_busy_count(struct ast_dsp *dsp, int cadences)
1255 {
1256         if (cadences < 1)
1257                 cadences = 1;
1258         if (cadences > DSP_HISTORY)
1259                 cadences = DSP_HISTORY;
1260         dsp->busycount = cadences;
1261 }
1262
1263 void ast_dsp_digitreset(struct ast_dsp *dsp)
1264 {
1265         int i;
1266         dsp->thinkdigit = 0;
1267         if (dsp->digitmode & DSP_DIGITMODE_MF) {
1268                 memset(dsp->td.mf.digits, 0, sizeof(dsp->td.mf.digits));
1269                 dsp->td.mf.current_digits = 0;
1270                 /* Reinitialise the detector for the next block */
1271                 for (i = 0;  i < 6;  i++) {
1272                 goertzel_reset(&dsp->td.mf.tone_out[i]);
1273                     goertzel_reset(&dsp->td.mf.tone_out2nd[i]);
1274                 }
1275                 dsp->td.mf.energy = 0.0;
1276                 dsp->td.mf.current_sample = 0;
1277             dsp->td.mf.hit1 = dsp->td.mf.hit2 = dsp->td.mf.hit3 = dsp->td.mf.hit4 = dsp->td.mf.mhit = 0;
1278         } else {
1279                 memset(dsp->td.dtmf.digits, 0, sizeof(dsp->td.dtmf.digits));
1280                 dsp->td.dtmf.current_digits = 0;
1281                 /* Reinitialise the detector for the next block */
1282                 for (i = 0;  i < 4;  i++) {
1283                 goertzel_reset(&dsp->td.dtmf.row_out[i]);
1284                     goertzel_reset(&dsp->td.dtmf.col_out[i]);
1285                 goertzel_reset(&dsp->td.dtmf.row_out2nd[i]);
1286                 goertzel_reset(&dsp->td.dtmf.col_out2nd[i]);
1287                 }
1288             goertzel_reset (&dsp->td.dtmf.fax_tone);
1289             goertzel_reset (&dsp->td.dtmf.fax_tone2nd);
1290                 dsp->td.dtmf.energy = 0.0;
1291                 dsp->td.dtmf.current_sample = 0;
1292             dsp->td.dtmf.hit1 = dsp->td.dtmf.hit2 = dsp->td.dtmf.hit3 = dsp->td.dtmf.hit4 = dsp->td.dtmf.mhit = 0;
1293         }
1294 }
1295
1296 void ast_dsp_reset(struct ast_dsp *dsp)
1297 {
1298         int x;
1299         dsp->totalsilence = 0;
1300         dsp->gsamps = 0;
1301         for (x=0;x<4;x++)
1302                 dsp->freqs[x].v2 = dsp->freqs[x].v3 = 0.0;
1303         memset(dsp->historicsilence, 0, sizeof(dsp->historicsilence));
1304         memset(dsp->historicnoise, 0, sizeof(dsp->historicnoise));
1305         
1306 }
1307
1308 int ast_dsp_digitmode(struct ast_dsp *dsp, int digitmode)
1309 {
1310         int new, old;
1311         old = dsp->digitmode & (DSP_DIGITMODE_DTMF | DSP_DIGITMODE_MF | DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX);
1312         new = digitmode & (DSP_DIGITMODE_DTMF | DSP_DIGITMODE_MF | DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX);
1313         if (old != new) {
1314                 /* Must initialize structures if switching from MF to DTMF or vice-versa */
1315                 if (new & DSP_DIGITMODE_MF)
1316                         ast_mf_detect_init(&dsp->td.mf);
1317                 else
1318                         ast_dtmf_detect_init(&dsp->td.dtmf);
1319         }
1320         dsp->digitmode = digitmode;
1321         return 0;
1322 }