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