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