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