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