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