Complete re-vamp of Radio Repeater application (app_rpt) and VERY minor changes in...
[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)
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 #ifdef OLD_DSP_ROUTINES
643                 if (!hit && (fax_energy >= FAX_THRESHOLD) && (fax_energy > s->energy * 21.0)) {
644                                 fax_energy_2nd = goertzel_result(&s->fax_tone2nd);
645                                 fax_energy_2nd = goertzel_result(&s->fax_tone2nd);
646                                 if (fax_energy_2nd * FAX_2ND_HARMONIC < fax_energy) {
647 #if 0
648                                         printf("Fax energy/Second Harmonic: %f/%f\n", fax_energy, fax_energy_2nd);
649 #endif                                  
650                                         /* XXX Probably need better checking than just this the energy XXX */
651                                         hit = 'f';
652                                         s->fax_hits++;
653                                 } /* Don't reset fax hits counter */
654                 }
655 #else /* OLD_DSP_ROUTINES */
656                 if (!hit && (fax_energy >= FAX_THRESHOLD) && (fax_energy >= DTMF_TO_TOTAL_ENERGY*s->energy)) {
657 #if 0
658                                 printf("Fax energy/Second Harmonic: %f\n", fax_energy);
659 #endif                                  
660                                         /* XXX Probably need better checking than just this the energy XXX */
661                                 hit = 'f';
662                                 s->fax_hits++;
663                 }
664 #endif /* OLD_DSP_ROUTINES */
665                 else {
666                         if (s->fax_hits > 5) {
667                                  hit = 'f';
668                                  s->mhit = 'f';
669                      s->detected_digits++;
670                      if (s->current_digits < MAX_DTMF_DIGITS)
671                      {
672                           s->digits[s->current_digits++] = hit;
673                           s->digits[s->current_digits] = '\0';
674                      }
675                      else
676                      {
677                            s->lost_digits++;
678                      }
679                         }
680                         s->fax_hits = 0;
681                 }
682 #endif /* FAX_DETECT */
683 #ifdef OLD_DSP_ROUTINES
684         s->hit1 = s->hit2;
685         s->hit2 = s->hit3;
686         s->hit3 = hit;
687 #else
688         s->hits[0] = s->hits[1];
689         s->hits[1] = s->hits[2];
690         s->hits[2] = hit;
691 #endif          
692         /* Reinitialise the detector for the next block */
693         for (i = 0;  i < 4;  i++)
694         {
695             goertzel_reset(&s->row_out[i]);
696             goertzel_reset(&s->col_out[i]);
697 #ifdef OLD_DSP_ROUTINES
698             goertzel_reset(&s->row_out2nd[i]);
699             goertzel_reset(&s->col_out2nd[i]);
700 #endif                  
701         }
702 #ifdef FAX_DETECT
703         goertzel_reset (&s->fax_tone);
704 #ifdef OLD_DSP_ROUTINES
705         goertzel_reset (&s->fax_tone2nd);
706 #endif                  
707 #endif
708                 s->energy = 0.0;
709         s->current_sample = 0;
710     }
711     if ((!s->mhit) || (s->mhit != hit))
712     {
713         s->mhit = 0;
714         return(0);
715     }
716     return (hit);
717 }
718
719 /* MF goertzel size */
720 #ifdef OLD_DSP_ROUTINES
721 #define MF_GSIZE 160
722 #else
723 #define MF_GSIZE 120
724 #endif
725
726 static int mf_detect (mf_detect_state_t *s,
727                  int16_t amp[],
728                  int samples, 
729                  int digitmode, int *writeback)
730 {
731
732 #ifdef OLD_DSP_ROUTINES
733     float tone_energy[6];
734     int best1;
735     int best2;
736         float max;
737         int sofarsogood;
738 #else
739     float energy[6];
740     int best;
741     int second_best;
742 #endif
743     float famp;
744     float v1;
745     int i;
746     int j;
747     int sample;
748     int hit;
749     int limit;
750
751     hit = 0;
752     for (sample = 0;  sample < samples;  sample = limit)
753     {
754         /* 80 is optimised to meet the MF specs. */
755         if ((samples - sample) >= (MF_GSIZE - s->current_sample))
756             limit = sample + (MF_GSIZE - s->current_sample);
757         else
758             limit = samples;
759 #if defined(USE_3DNOW)
760         _dtmf_goertzel_update (s->row_out, amp + sample, limit - sample);
761         _dtmf_goertzel_update (s->col_out, amp + sample, limit - sample);
762 #ifdef OLD_DSP_ROUTINES
763         _dtmf_goertzel_update (s->row_out2nd, amp + sample, limit2 - sample);
764         _dtmf_goertzel_update (s->col_out2nd, amp + sample, limit2 - sample);
765 #endif
766                 /* XXX Need to fax detect for 3dnow too XXX */
767                 #warning "Fax Support Broken"
768 #else
769         /* The following unrolled loop takes only 35% (rough estimate) of the 
770            time of a rolled loop on the machine on which it was developed */
771         for (j = sample;  j < limit;  j++)
772         {
773             famp = amp[j];
774             
775 #ifdef OLD_DSP_ROUTINES
776             s->energy += famp*famp;
777 #endif
778             
779             /* With GCC 2.95, the following unrolled code seems to take about 35%
780                (rough estimate) as long as a neat little 0-3 loop */
781             v1 = s->tone_out[0].v2;
782             s->tone_out[0].v2 = s->tone_out[0].v3;
783             s->tone_out[0].v3 = s->tone_out[0].fac*s->tone_out[0].v2 - v1 + famp;
784
785             v1 = s->tone_out[1].v2;
786             s->tone_out[1].v2 = s->tone_out[1].v3;
787             s->tone_out[1].v3 = s->tone_out[1].fac*s->tone_out[1].v2 - v1 + famp;
788     
789             v1 = s->tone_out[2].v2;
790             s->tone_out[2].v2 = s->tone_out[2].v3;
791             s->tone_out[2].v3 = s->tone_out[2].fac*s->tone_out[2].v2 - v1 + famp;
792     
793             v1 = s->tone_out[3].v2;
794             s->tone_out[3].v2 = s->tone_out[3].v3;
795             s->tone_out[3].v3 = s->tone_out[3].fac*s->tone_out[3].v2 - v1 + famp;
796
797             v1 = s->tone_out[4].v2;
798             s->tone_out[4].v2 = s->tone_out[4].v3;
799             s->tone_out[4].v3 = s->tone_out[4].fac*s->tone_out[4].v2 - v1 + famp;
800
801             v1 = s->tone_out[5].v2;
802             s->tone_out[5].v2 = s->tone_out[5].v3;
803             s->tone_out[5].v3 = s->tone_out[5].fac*s->tone_out[5].v2 - v1 + famp;
804
805 #ifdef OLD_DSP_ROUTINES
806             v1 = s->tone_out2nd[0].v2;
807             s->tone_out2nd[0].v2 = s->tone_out2nd[0].v3;
808             s->tone_out2nd[0].v3 = s->tone_out2nd[0].fac*s->tone_out2nd[0].v2 - v1 + famp;
809         
810             v1 = s->tone_out2nd[1].v2;
811             s->tone_out2nd[1].v2 = s->tone_out2nd[1].v3;
812             s->tone_out2nd[1].v3 = s->tone_out2nd[1].fac*s->tone_out2nd[1].v2 - v1 + famp;
813         
814             v1 = s->tone_out2nd[2].v2;
815             s->tone_out2nd[2].v2 = s->tone_out2nd[2].v3;
816             s->tone_out2nd[2].v3 = s->tone_out2nd[2].fac*s->tone_out2nd[2].v2 - v1 + famp;
817         
818             v1 = s->tone_out2nd[3].v2;
819             s->tone_out2nd[3].v2 = s->tone_out2nd[3].v3;
820             s->tone_out2nd[3].v3 = s->tone_out2nd[3].fac*s->tone_out2nd[3].v2 - v1 + famp;
821
822             v1 = s->tone_out2nd[4].v2;
823             s->tone_out2nd[4].v2 = s->tone_out2nd[4].v3;
824             s->tone_out2nd[4].v3 = s->tone_out2nd[4].fac*s->tone_out2nd[2].v2 - v1 + famp;
825         
826             v1 = s->tone_out2nd[3].v2;
827             s->tone_out2nd[5].v2 = s->tone_out2nd[6].v3;
828             s->tone_out2nd[5].v3 = s->tone_out2nd[6].fac*s->tone_out2nd[3].v2 - v1 + famp;
829 #endif
830         }
831 #endif
832         s->current_sample += (limit - sample);
833         if (s->current_sample < MF_GSIZE) {
834                         if (hit && !((digitmode & DSP_DIGITMODE_NOQUELCH))) {
835                                 /* If we had a hit last time, go ahead and clear this out since likely it
836                                    will be another hit */
837                                 for (i=sample;i<limit;i++) 
838                                         amp[i] = 0;
839                                 *writeback = 1;
840                         }
841             continue;
842                 }
843
844
845 #ifdef OLD_DSP_ROUTINES         
846                 /* We're at the end of an MF detection block.  Go ahead and calculate
847                    all the energies. */
848                 for (i=0;i<6;i++) {
849                         tone_energy[i] = goertzel_result(&s->tone_out[i]);
850                 }
851                 /* Find highest */
852                 best1 = 0;
853                 max = tone_energy[0];
854                 for (i=1;i<6;i++) {
855                         if (tone_energy[i] > max) {
856                                 max = tone_energy[i];
857                                 best1 = i;
858                         }
859                 }
860
861                 /* Find 2nd highest */
862                 if (best1) {
863                         max = tone_energy[0];
864                         best2 = 0;
865                 } else {
866                         max = tone_energy[1];
867                         best2 = 1;
868                 }
869
870                 for (i=0;i<6;i++) {
871                         if (i == best1) continue;
872                         if (tone_energy[i] > max) {
873                                 max = tone_energy[i];
874                                 best2 = i;
875                         }
876                 }
877                                 
878         hit = 0;
879                 if (best1 != best2) sofarsogood=1;
880                 else sofarsogood=0;
881                 /* Check for relative energies */
882                 for (i=0;i<6;i++) {
883                         if (i == best1) continue;
884                         if (i == best2) continue;
885                         if (tone_energy[best1] < tone_energy[i] * MF_RELATIVE_PEAK) {
886                                 sofarsogood = 0;
887                                 break;
888                         }
889                         if (tone_energy[best2] < tone_energy[i] * MF_RELATIVE_PEAK) {
890                                 sofarsogood = 0;
891                                 break;
892                         }
893                 }
894                 
895                 if (sofarsogood) {
896                         /* Check for 2nd harmonic */
897                         if (goertzel_result(&s->tone_out2nd[best1]) * MF_2ND_HARMONIC > tone_energy[best1]) 
898                                 sofarsogood = 0;
899                         else if (goertzel_result(&s->tone_out2nd[best2]) * MF_2ND_HARMONIC > tone_energy[best2])
900                                 sofarsogood = 0;
901                 }
902                 if (sofarsogood) {
903                         hit = mf_hit[best1][best2];
904                         if (!(digitmode & DSP_DIGITMODE_NOQUELCH)) {
905                                 /* Zero out frame data if this is part DTMF */
906                                 for (i=sample;i<limit;i++) 
907                                         amp[i] = 0;
908                                 *writeback = 1;
909                         }
910                         /* Look for two consecutive clean hits */
911                         if ((hit == s->hit3) && (s->hit3 != s->hit2)) {
912                                 s->mhit = hit;
913                                 s->detected_digits++;
914                                 if (s->current_digits < MAX_DTMF_DIGITS - 2) {
915                                         s->digits[s->current_digits++] = hit;
916                                         s->digits[s->current_digits] = '\0';
917                                 } else {
918                                         s->lost_digits++;
919                                 }
920                         }
921                 }
922                 
923         s->hit1 = s->hit2;
924         s->hit2 = s->hit3;
925         s->hit3 = hit;
926         /* Reinitialise the detector for the next block */
927         for (i = 0;  i < 6;  i++)
928         {
929             goertzel_reset(&s->tone_out[i]);
930             goertzel_reset(&s->tone_out2nd[i]);
931         }
932                 s->energy = 0.0;
933         s->current_sample = 0;
934     }
935 #else
936                 /* We're at the end of an MF detection block.  */
937         /* Find the two highest energies. The spec says to look for
938            two tones and two tones only. Taking this literally -ie
939            only two tones pass the minimum threshold - doesn't work
940            well. The sinc function mess, due to rectangular windowing
941            ensure that! Find the two highest energies and ensure they
942            are considerably stronger than any of the others. */
943         energy[0] = goertzel_result(&s->tone_out[0]);
944         energy[1] = goertzel_result(&s->tone_out[1]);
945         if (energy[0] > energy[1])
946         {
947             best = 0;
948             second_best = 1;
949         }
950         else
951         {
952             best = 1;
953             second_best = 0;
954         }
955         /*endif*/
956         for (i = 2;  i < 6;  i++)
957         {
958             energy[i] = goertzel_result(&s->tone_out[i]);
959             if (energy[i] >= energy[best])
960             {
961                 second_best = best;
962                 best = i;
963             }
964             else if (energy[i] >= energy[second_best])
965             {
966                 second_best = i;
967             }
968         }
969         /* Basic signal level and twist tests */
970         hit = 0;
971         if (energy[best] >= BELL_MF_THRESHOLD
972             &&
973             energy[second_best] >= BELL_MF_THRESHOLD
974             &&
975             energy[best] < energy[second_best]*BELL_MF_TWIST
976             &&
977             energy[best]*BELL_MF_TWIST > energy[second_best])
978         {
979             /* Relative peak test */
980             hit = -1;
981             for (i = 0;  i < 6;  i++)
982             {
983                 if (i != best  &&  i != second_best)
984                 {
985                     if (energy[i]*BELL_MF_RELATIVE_PEAK >= energy[second_best])
986                     {
987                         /* The best two are not clearly the best */
988                         hit = 0;
989                         break;
990                     }
991                 }
992             }
993         }
994         if (hit)
995         {
996             /* Get the values into ascending order */
997             if (second_best < best)
998             {
999                 i = best;
1000                 best = second_best;
1001                 second_best = i;
1002             }
1003             best = best*5 + second_best - 1;
1004             hit = bell_mf_positions[best];
1005             /* Look for two successive similar results */
1006             /* The logic in the next test is:
1007                For KP we need 4 successive identical clean detects, with
1008                two blocks of something different preceeding it. For anything
1009                else we need two successive identical clean detects, with
1010                two blocks of something different preceeding it. */
1011             if (hit == s->hits[4]
1012                 &&
1013                 hit == s->hits[3]
1014                 &&
1015                    ((hit != '*'  &&  hit != s->hits[2]  &&  hit != s->hits[1])
1016                     ||
1017                     (hit == '*'  &&  hit == s->hits[2]  &&  hit != s->hits[1]  &&  hit != s->hits[0])))
1018             {
1019                 s->detected_digits++;
1020                 if (s->current_digits < MAX_DTMF_DIGITS)
1021                 {
1022                     s->digits[s->current_digits++] = hit;
1023                     s->digits[s->current_digits] = '\0';
1024                 }
1025                 else
1026                 {
1027                     s->lost_digits++;
1028                 }
1029             }
1030         }
1031         else
1032         {
1033             hit = 0;
1034         }
1035         s->hits[0] = s->hits[1];
1036         s->hits[1] = s->hits[2];
1037         s->hits[2] = s->hits[3];
1038         s->hits[3] = s->hits[4];
1039         s->hits[4] = hit;
1040         /* Reinitialise the detector for the next block */
1041         for (i = 0;  i < 6;  i++)
1042             goertzel_reset(&s->tone_out[i]);
1043         s->current_sample = 0;
1044     }
1045 #endif  
1046     if ((!s->mhit) || (s->mhit != hit))
1047     {
1048                 s->mhit = 0;
1049                 return(0);
1050     }
1051     return (hit);
1052 }
1053
1054 static int __ast_dsp_digitdetect(struct ast_dsp *dsp, short *s, int len, int *writeback)
1055 {
1056         int res;
1057         if (dsp->digitmode & DSP_DIGITMODE_MF)
1058                 res = mf_detect(&dsp->td.mf, s, len, dsp->digitmode & DSP_DIGITMODE_RELAXDTMF, writeback);
1059         else
1060                 res = dtmf_detect(&dsp->td.dtmf, s, len, dsp->digitmode & DSP_DIGITMODE_RELAXDTMF, writeback);
1061         return res;
1062 }
1063
1064 int ast_dsp_digitdetect(struct ast_dsp *dsp, struct ast_frame *inf)
1065 {
1066         short *s;
1067         int len;
1068         int ign=0;
1069         if (inf->frametype != AST_FRAME_VOICE) {
1070                 ast_log(LOG_WARNING, "Can't check call progress of non-voice frames\n");
1071                 return 0;
1072         }
1073         if (inf->subclass != AST_FORMAT_SLINEAR) {
1074                 ast_log(LOG_WARNING, "Can only check call progress in signed-linear frames\n");
1075                 return 0;
1076         }
1077         s = inf->data;
1078         len = inf->datalen / 2;
1079         return __ast_dsp_digitdetect(dsp, s, len, &ign);
1080 }
1081
1082 static inline int pair_there(float p1, float p2, float i1, float i2, float e)
1083 {
1084         /* See if p1 and p2 are there, relative to i1 and i2 and total energy */
1085         /* Make sure absolute levels are high enough */
1086         if ((p1 < TONE_MIN_THRESH) || (p2 < TONE_MIN_THRESH))
1087                 return 0;
1088         /* Amplify ignored stuff */
1089         i2 *= TONE_THRESH;
1090         i1 *= TONE_THRESH;
1091         e *= TONE_THRESH;
1092         /* Check first tone */
1093         if ((p1 < i1) || (p1 < i2) || (p1 < e))
1094                 return 0;
1095         /* And second */
1096         if ((p2 < i1) || (p2 < i2) || (p2 < e))
1097                 return 0;
1098         /* Guess it's there... */
1099         return 1;
1100 }
1101
1102 int ast_dsp_getdigits (struct ast_dsp *dsp,
1103               char *buf,
1104               int max)
1105 {
1106         if (dsp->digitmode & DSP_DIGITMODE_MF) {
1107             if (max > dsp->td.mf.current_digits)
1108                 max = dsp->td.mf.current_digits;
1109             if (max > 0)
1110             {
1111                 memcpy (buf, dsp->td.mf.digits, max);
1112                 memmove (dsp->td.mf.digits, dsp->td.mf.digits + max, dsp->td.mf.current_digits - max);
1113                 dsp->td.mf.current_digits -= max;
1114             }
1115             buf[max] = '\0';
1116             return  max;
1117         } else {
1118             if (max > dsp->td.dtmf.current_digits)
1119                 max = dsp->td.dtmf.current_digits;
1120             if (max > 0)
1121             {
1122                 memcpy (buf, dsp->td.dtmf.digits, max);
1123                 memmove (dsp->td.dtmf.digits, dsp->td.dtmf.digits + max, dsp->td.dtmf.current_digits - max);
1124                 dsp->td.dtmf.current_digits -= max;
1125             }
1126             buf[max] = '\0';
1127             return  max;
1128         }
1129 }
1130
1131 static int __ast_dsp_call_progress(struct ast_dsp *dsp, short *s, int len)
1132 {
1133         int x;
1134         int y;
1135         int pass;
1136         int newstate = TONE_STATE_SILENCE;
1137         int res = 0;
1138         while(len) {
1139                 /* Take the lesser of the number of samples we need and what we have */
1140                 pass = len;
1141                 if (pass > dsp->gsamp_size - dsp->gsamps) 
1142                         pass = dsp->gsamp_size - dsp->gsamps;
1143                 for (x=0;x<pass;x++) {
1144                         for (y=0;y<dsp->freqcount;y++) 
1145                                 goertzel_sample(&dsp->freqs[y], s[x]);
1146                         dsp->genergy += s[x] * s[x];
1147                 }
1148                 s += pass;
1149                 dsp->gsamps += pass;
1150                 len -= pass;
1151                 if (dsp->gsamps == dsp->gsamp_size) {
1152                         float hz[7];
1153                         for (y=0;y<7;y++)
1154                                 hz[y] = goertzel_result(&dsp->freqs[y]);
1155 #if 0
1156                         printf("Got whole dsp state: 350: %e, 440: %e, 480: %e, 620: %e, 950: %e, 1400: %e, 1800: %e, Energy: %e\n", 
1157                                 hz_350, hz_440, hz_480, hz_620, hz_950, hz_1400, hz_1800, dsp->genergy);
1158 #endif
1159                         switch(dsp->progmode) {
1160                         case PROG_MODE_NA:
1161                                 if (pair_there(hz[HZ_480], hz[HZ_620], hz[HZ_350], hz[HZ_440], dsp->genergy)) {
1162                                         newstate = TONE_STATE_BUSY;
1163                                 } else if (pair_there(hz[HZ_440], hz[HZ_480], hz[HZ_350], hz[HZ_620], dsp->genergy)) {
1164                                         newstate = TONE_STATE_RINGING;
1165                                 } else if (pair_there(hz[HZ_350], hz[HZ_440], hz[HZ_480], hz[HZ_620], dsp->genergy)) {
1166                                         newstate = TONE_STATE_DIALTONE;
1167                                 } else if (hz[HZ_950] > TONE_MIN_THRESH * TONE_THRESH) {
1168                                         newstate = TONE_STATE_SPECIAL1;
1169                                 } else if (hz[HZ_1400] > TONE_MIN_THRESH * TONE_THRESH) {
1170                                         if (dsp->tstate == TONE_STATE_SPECIAL1)
1171                                                 newstate = TONE_STATE_SPECIAL2;
1172                                 } else if (hz[HZ_1800] > TONE_MIN_THRESH * TONE_THRESH) {
1173                                         if (dsp->tstate == TONE_STATE_SPECIAL2)
1174                                                 newstate = TONE_STATE_SPECIAL3;
1175                                 } else if (dsp->genergy > TONE_MIN_THRESH * TONE_THRESH) {
1176                                         newstate = TONE_STATE_TALKING;
1177                                 } else
1178                                         newstate = TONE_STATE_SILENCE;
1179                                 break;
1180                         case PROG_MODE_CR:
1181                                 if (hz[HZ_425] > TONE_MIN_THRESH * TONE_THRESH) {
1182                                         newstate = TONE_STATE_RINGING;
1183                                 } else if (dsp->genergy > TONE_MIN_THRESH * TONE_THRESH) {
1184                                         newstate = TONE_STATE_TALKING;
1185                                 } else
1186                                         newstate = TONE_STATE_SILENCE;
1187                                 break;
1188                         default:
1189                                 ast_log(LOG_WARNING, "Can't process in unknown prog mode '%d'\n", dsp->progmode);
1190                         }
1191                         if (newstate == dsp->tstate) {
1192                                 dsp->tcount++;
1193                                 if (dsp->tcount == COUNT_THRESH) {
1194                                         if (dsp->tstate == TONE_STATE_BUSY) {
1195                                                 res = AST_CONTROL_BUSY;
1196                                                 dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
1197                                         } else if (dsp->tstate == TONE_STATE_TALKING) {
1198                                                 res = AST_CONTROL_ANSWER;
1199                                                 dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
1200                                         } else if (dsp->tstate == TONE_STATE_RINGING)
1201                                                 res = AST_CONTROL_RINGING;
1202                                         else if (dsp->tstate == TONE_STATE_SPECIAL3) {
1203                                                 res = AST_CONTROL_CONGESTION;
1204                                                 dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
1205                                         }
1206                                         
1207                                 }
1208                         } else {
1209 #if 0
1210                                 printf("Newstate: %d\n", newstate);
1211 #endif
1212                                 dsp->tstate = newstate;
1213                                 dsp->tcount = 1;
1214                         }
1215                         
1216                         /* Reset goertzel */                                            
1217                         for (x=0;x<7;x++)
1218                                 dsp->freqs[x].v2 = dsp->freqs[x].v3 = 0.0;
1219                         dsp->gsamps = 0;
1220                         dsp->genergy = 0.0;
1221                 }
1222         }
1223 #if 0
1224         if (res)
1225                 printf("Returning %d\n", res);
1226 #endif          
1227         return res;
1228 }
1229
1230 int ast_dsp_call_progress(struct ast_dsp *dsp, struct ast_frame *inf)
1231 {
1232         if (inf->frametype != AST_FRAME_VOICE) {
1233                 ast_log(LOG_WARNING, "Can't check call progress of non-voice frames\n");
1234                 return 0;
1235         }
1236         if (inf->subclass != AST_FORMAT_SLINEAR) {
1237                 ast_log(LOG_WARNING, "Can only check call progress in signed-linear frames\n");
1238                 return 0;
1239         }
1240         return __ast_dsp_call_progress(dsp, inf->data, inf->datalen / 2);
1241 }
1242
1243 static int __ast_dsp_silence(struct ast_dsp *dsp, short *s, int len, int *totalsilence)
1244 {
1245         int accum;
1246         int x;
1247         int res = 0;
1248         
1249         accum = 0;
1250         for (x=0;x<len; x++) 
1251                 accum += abs(s[x]);
1252         accum /= len;
1253         if (accum < dsp->threshold) {
1254                 dsp->totalsilence += len/8;
1255                 if (dsp->totalnoise) {
1256                         /* Move and save history */
1257                         memmove(dsp->historicnoise + DSP_HISTORY - dsp->busycount, dsp->historicnoise + DSP_HISTORY - dsp->busycount +1, dsp->busycount*sizeof(dsp->historicnoise[0]));
1258                         dsp->historicnoise[DSP_HISTORY - 1] = dsp->totalnoise;
1259 /* we don't want to check for busydetect that frequently */
1260 #if 0
1261                         dsp->busymaybe = 1;
1262 #endif
1263                 }
1264                 dsp->totalnoise = 0;
1265                 res = 1;
1266         } else {
1267                 dsp->totalnoise += len/8;
1268                 if (dsp->totalsilence) {
1269                         int silence1 = dsp->historicsilence[DSP_HISTORY - 1];
1270                         int silence2 = dsp->historicsilence[DSP_HISTORY - 2];
1271                         /* Move and save history */
1272                         memmove(dsp->historicsilence + DSP_HISTORY - dsp->busycount, dsp->historicsilence + DSP_HISTORY - dsp->busycount + 1, dsp->busycount*sizeof(dsp->historicsilence[0]));
1273                         dsp->historicsilence[DSP_HISTORY - 1] = dsp->totalsilence;
1274                         /* check if the previous sample differs only by BUSY_PERCENT from the one before it */
1275                         if (silence1 < silence2) {
1276                                 if (silence1 + silence1/BUSY_PERCENT >= silence2)
1277                                         dsp->busymaybe = 1;
1278                                 else 
1279                                         dsp->busymaybe = 0;
1280                         } else {
1281                                 if (silence1 - silence1/BUSY_PERCENT <= silence2)
1282                                         dsp->busymaybe = 1;
1283                                 else 
1284                                         dsp->busymaybe = 0;
1285                         }
1286                                         
1287                 }
1288                 dsp->totalsilence = 0;
1289         }
1290         if (totalsilence)
1291                 *totalsilence = dsp->totalsilence;
1292         return res;
1293 }
1294 #ifdef BUSYDETECT_MARTIN
1295 int ast_dsp_busydetect(struct ast_dsp *dsp)
1296 {
1297         int res = 0, x;
1298 #ifndef BUSYDETECT_TONEONLY
1299         int avgsilence = 0, hitsilence = 0;
1300 #endif
1301         int avgtone = 0, hittone = 0;
1302         if (!dsp->busymaybe)
1303                 return res;
1304         for (x=DSP_HISTORY - dsp->busycount;x<DSP_HISTORY;x++) {
1305 #ifndef BUSYDETECT_TONEONLY
1306                 avgsilence += dsp->historicsilence[x];
1307 #endif
1308                 avgtone += dsp->historicnoise[x];
1309         }
1310 #ifndef BUSYDETECT_TONEONLY
1311         avgsilence /= dsp->busycount;
1312 #endif
1313         avgtone /= dsp->busycount;
1314         for (x=DSP_HISTORY - dsp->busycount;x<DSP_HISTORY;x++) {
1315 #ifndef BUSYDETECT_TONEONLY
1316                 if (avgsilence > dsp->historicsilence[x]) {
1317                         if (avgsilence - (avgsilence / BUSY_PERCENT) <= dsp->historicsilence[x])
1318                                 hitsilence++;
1319                 } else {
1320                         if (avgsilence + (avgsilence / BUSY_PERCENT) >= dsp->historicsilence[x])
1321                                 hitsilence++;
1322                 }
1323 #endif
1324                 if (avgtone > dsp->historicnoise[x]) {
1325                         if (avgtone - (avgtone / BUSY_PERCENT) <= dsp->historicsilence[x])
1326                                 hittone++;
1327                 } else {
1328                         if (avgtone + (avgtone / BUSY_PERCENT) >= dsp->historicsilence[x])
1329                                 hittone++;
1330                 }
1331         }
1332 #ifndef BUSYDETECT_TONEONLY
1333         if ((hittone >= dsp->busycount - 1) && (hitsilence >= dsp->busycount - 1) && (avgtone >= BUSY_MIN && avgtone <= BUSY_MAX) && (avgsilence >= BUSY_MIN && avgsilence <= BUSY_MAX)) {
1334 #else
1335         if ((hittone >= dsp->busycount - 1) && (avgtone >= BUSY_MIN && avgtone <= BUSY_MAX)) {
1336 #endif
1337 #ifdef BUSYDETECT_COMPARE_TONE_AND_SILENCE
1338 #ifdef BUSYDETECT_TONEONLY
1339 #error You cant use BUSYDETECT_TONEONLY together with BUSYDETECT_COMPARE_TONE_AND_SILENCE
1340 #endif
1341                 if (avgtone > avgsilence) {
1342                         if (avgtone - avgtone/(BUSY_PERCENT*2) <= avgsilence)
1343                                 res = 1;
1344                 } else {
1345                         if (avgtone + avgtone/(BUSY_PERCENT*2) >= avgsilence)
1346                                 res = 1;
1347                 }
1348 #else
1349                 res = 1;
1350 #endif
1351         }
1352 #if 0
1353         if (res)
1354                 ast_log(LOG_NOTICE, "detected busy, avgtone: %d, avgsilence %d\n", avgtone, avgsilence);
1355 #endif
1356         return res;
1357 }
1358 #endif
1359
1360 #ifdef BUSYDETECT
1361 int ast_dsp_busydetect(struct ast_dsp *dsp)
1362 {
1363         int x;
1364         int res = 0;
1365         int max, min;
1366
1367 #if 0
1368         if (dsp->busy_hits > 5);
1369         return 0;
1370 #endif
1371         if (dsp->busymaybe) {
1372 #if 0
1373                 printf("Maybe busy!\n");
1374 #endif          
1375                 dsp->busymaybe = 0;
1376                 min = 9999;
1377                 max = 0;
1378                 for (x=DSP_HISTORY - dsp->busycount;x<DSP_HISTORY;x++) {
1379 #if 0
1380                         printf("Silence: %d, Noise: %d\n", dsp->historicsilence[x], dsp->historicnoise[x]);
1381 #endif                  
1382                         if (dsp->historicsilence[x] < min)
1383                                 min = dsp->historicsilence[x];
1384                         if (dsp->historicnoise[x] < min)
1385                                 min = dsp->historicnoise[x];
1386                         if (dsp->historicsilence[x] > max)
1387                                 max = dsp->historicsilence[x];
1388                         if (dsp->historicnoise[x] > max)
1389                                 max = dsp->historicnoise[x];
1390                 }
1391                 if ((max - min < BUSY_THRESHOLD) && (max < BUSY_MAX) && (min > BUSY_MIN)) {
1392 #if 0
1393                         printf("Busy!\n");
1394 #endif                  
1395                         res = 1;
1396                 }
1397 #if 0
1398                 printf("Min: %d, max: %d\n", min, max);
1399 #endif          
1400         }
1401         return res;
1402 }
1403 #endif
1404
1405 int ast_dsp_silence(struct ast_dsp *dsp, struct ast_frame *f, int *totalsilence)
1406 {
1407         short *s;
1408         int len;
1409         
1410         if (f->frametype != AST_FRAME_VOICE) {
1411                 ast_log(LOG_WARNING, "Can't calculate silence on a non-voice frame\n");
1412                 return 0;
1413         }
1414         if (f->subclass != AST_FORMAT_SLINEAR) {
1415                 ast_log(LOG_WARNING, "Can only calculate silence on signed-linear frames :(\n");
1416                 return 0;
1417         }
1418         s = f->data;
1419         len = f->datalen/2;
1420         return __ast_dsp_silence(dsp, s, len, totalsilence);
1421 }
1422
1423 struct ast_frame *ast_dsp_process(struct ast_channel *chan, struct ast_dsp *dsp, struct ast_frame *af)
1424 {
1425         int silence;
1426         int res;
1427         int digit;
1428         int x;
1429         unsigned short *shortdata;
1430         unsigned char *odata;
1431         int len;
1432         int writeback = 0;
1433
1434 #define FIX_INF(inf) do { \
1435                 if (writeback) { \
1436                         switch(inf->subclass) { \
1437                         case AST_FORMAT_SLINEAR: \
1438                                 break; \
1439                         case AST_FORMAT_ULAW: \
1440                                 for (x=0;x<len;x++) \
1441                                         odata[x] = AST_LIN2MU(shortdata[x]); \
1442                                 break; \
1443                         case AST_FORMAT_ALAW: \
1444                                 for (x=0;x<len;x++) \
1445                                         odata[x] = AST_LIN2A(shortdata[x]); \
1446                                 break; \
1447                         } \
1448                 } \
1449         } while(0) 
1450
1451         if (!af)
1452                 return NULL;
1453         if (af->frametype != AST_FRAME_VOICE)
1454                 return af;
1455         odata = af->data;
1456         len = af->datalen;
1457         /* Make sure we have short data */
1458         switch(af->subclass) {
1459         case AST_FORMAT_SLINEAR:
1460                 shortdata = af->data;
1461                 len = af->datalen / 2;
1462                 break;
1463         case AST_FORMAT_ULAW:
1464                 shortdata = alloca(af->datalen * 2);
1465                 if (!shortdata) {
1466                         ast_log(LOG_WARNING, "Unable to allocate stack space for data: %s\n", strerror(errno));
1467                         return af;
1468                 }
1469                 for (x=0;x<len;x++) 
1470                         shortdata[x] = AST_MULAW(odata[x]);
1471                 break;
1472         case AST_FORMAT_ALAW:
1473                 shortdata = alloca(af->datalen * 2);
1474                 if (!shortdata) {
1475                         ast_log(LOG_WARNING, "Unable to allocate stack space for data: %s\n", strerror(errno));
1476                         return af;
1477                 }
1478                 for (x=0;x<len;x++) 
1479                         shortdata[x] = AST_ALAW(odata[x]);
1480                 break;
1481         default:
1482                 ast_log(LOG_WARNING, "Unable to process inband DTMF on %d frames\n", af->subclass);
1483                 return af;
1484         }
1485         silence = __ast_dsp_silence(dsp, shortdata, len, NULL);
1486         if ((dsp->features & DSP_FEATURE_SILENCE_SUPPRESS) && silence) {
1487                 memset(&dsp->f, 0, sizeof(dsp->f));
1488                 dsp->f.frametype = AST_FRAME_NULL;
1489                 return &dsp->f;
1490         }
1491         if ((dsp->features & DSP_FEATURE_BUSY_DETECT) && ast_dsp_busydetect(dsp)) {
1492                 chan->_softhangup |= AST_SOFTHANGUP_DEV;
1493                 memset(&dsp->f, 0, sizeof(dsp->f));
1494                 dsp->f.frametype = AST_FRAME_CONTROL;
1495                 dsp->f.subclass = AST_CONTROL_BUSY;
1496                 ast_log(LOG_DEBUG, "Requesting Hangup because the busy tone was detected on channel %s\n", chan->name);
1497                 return &dsp->f;
1498         }
1499         if ((dsp->features & DSP_FEATURE_DTMF_DETECT)) {
1500                 digit = __ast_dsp_digitdetect(dsp, shortdata, len, &writeback);
1501 #if 0
1502                 if (digit)
1503                         printf("Performing digit detection returned %d, digitmode is %d\n", digit, dsp->digitmode);
1504 #endif                  
1505                 if (dsp->digitmode & (DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX)) {
1506                         if (!dsp->thinkdigit) {
1507                                 if (digit) {
1508                                         /* Looks like we might have something.  Request a conference mute for the moment */
1509                                         memset(&dsp->f, 0, sizeof(dsp->f));
1510                                         dsp->f.frametype = AST_FRAME_DTMF;
1511                                         dsp->f.subclass = 'm';
1512                                         dsp->thinkdigit = 'x';
1513                                         FIX_INF(af);
1514                                         if (chan)
1515                                                 ast_queue_frame(chan, af);
1516                                         ast_frfree(af);
1517                                         return &dsp->f;
1518                                 }
1519                         } else {
1520                                 if (digit) {
1521                                         /* Thought we saw one last time.  Pretty sure we really have now */
1522                                         if (dsp->thinkdigit) {
1523                                                 if ((dsp->thinkdigit != 'x') && (dsp->thinkdigit != digit)) {
1524                                                         /* If we found a digit, and we're changing digits, go
1525                                                            ahead and send this one, but DON'T stop confmute because
1526                                                            we're detecting something else, too... */
1527                                                         memset(&dsp->f, 0, sizeof(dsp->f));
1528                                                         dsp->f.frametype = AST_FRAME_DTMF;
1529                                                         dsp->f.subclass = dsp->thinkdigit;
1530                                                         FIX_INF(af);
1531                                                         if (chan)
1532                                                                 ast_queue_frame(chan, af);
1533                                                         ast_frfree(af);
1534                                                 }
1535                                                 dsp->thinkdigit = digit;
1536                                                 return &dsp->f;
1537                                         }
1538                                         dsp->thinkdigit = digit;
1539                                 } else {
1540                                         if (dsp->thinkdigit) {
1541                                                 memset(&dsp->f, 0, sizeof(dsp->f));
1542                                                 if (dsp->thinkdigit != 'x') {
1543                                                         /* If we found a digit, send it now */
1544                                                         dsp->f.frametype = AST_FRAME_DTMF;
1545                                                         dsp->f.subclass = dsp->thinkdigit;
1546                                                         dsp->thinkdigit = 0;
1547                                                 } else {
1548                                                         dsp->f.frametype = AST_FRAME_DTMF;
1549                                                         dsp->f.subclass = 'u';
1550                                                         dsp->thinkdigit = 0;
1551                                                 }
1552                                                 FIX_INF(af);
1553                                                 if (chan)
1554                                                         ast_queue_frame(chan, af);
1555                                                 ast_frfree(af);
1556                                                 return &dsp->f;
1557                                         }
1558                                 }
1559                         }
1560                 } else if (!digit) {
1561                         /* Only check when there is *not* a hit... */
1562                         if (dsp->digitmode & DSP_DIGITMODE_MF) {
1563                                 if (dsp->td.mf.current_digits) {
1564                                         memset(&dsp->f, 0, sizeof(dsp->f));
1565                                         dsp->f.frametype = AST_FRAME_DTMF;
1566                                         dsp->f.subclass = dsp->td.mf.digits[0];
1567                                         memmove(dsp->td.mf.digits, dsp->td.mf.digits + 1, dsp->td.mf.current_digits);
1568                                         dsp->td.mf.current_digits--;
1569                                         FIX_INF(af);
1570                                         if (chan)
1571                                                 ast_queue_frame(chan, af);
1572                                         ast_frfree(af);
1573                                         return &dsp->f;
1574                                 }
1575                         } else {
1576                                 if (dsp->td.dtmf.current_digits) {
1577                                         memset(&dsp->f, 0, sizeof(dsp->f));
1578                                         dsp->f.frametype = AST_FRAME_DTMF;
1579                                         dsp->f.subclass = dsp->td.dtmf.digits[0];
1580                                         memmove(dsp->td.dtmf.digits, dsp->td.dtmf.digits + 1, dsp->td.dtmf.current_digits);
1581                                         dsp->td.dtmf.current_digits--;
1582                                         FIX_INF(af);
1583                                         if (chan)
1584                                                 ast_queue_frame(chan, af);
1585                                         ast_frfree(af);
1586                                         return &dsp->f;
1587                                 }
1588                         }
1589                 }
1590         }
1591         if ((dsp->features & DSP_FEATURE_CALL_PROGRESS)) {
1592                 res = __ast_dsp_call_progress(dsp, shortdata, len);
1593                 memset(&dsp->f, 0, sizeof(dsp->f));
1594                 dsp->f.frametype = AST_FRAME_CONTROL;
1595                 if (res) {
1596                         switch(res) {
1597                         case AST_CONTROL_ANSWER:
1598                         case AST_CONTROL_BUSY:
1599                         case AST_CONTROL_RINGING:
1600                         case AST_CONTROL_CONGESTION:
1601                                 dsp->f.subclass = res;
1602                                 if (chan) 
1603                                         ast_queue_frame(chan, &dsp->f);
1604                                 break;
1605                         default:
1606                                 ast_log(LOG_WARNING, "Don't know how to represent call progress message %d\n", res);
1607                         }
1608                 }
1609         }
1610         FIX_INF(af);
1611         return af;
1612 }
1613
1614 static void ast_dsp_prog_reset(struct ast_dsp *dsp)
1615 {
1616         int max = 0;
1617         int x;
1618         dsp->gsamp_size = modes[dsp->progmode].size;
1619         dsp->gsamps = 0;
1620         for (x=0;x<sizeof(modes[dsp->progmode].freqs) / sizeof(modes[dsp->progmode].freqs[0]);x++) {
1621                 if (modes[dsp->progmode].freqs[x]) {
1622                         goertzel_init(&dsp->freqs[x], (float)modes[dsp->progmode].freqs[x], dsp->gsamp_size);
1623                         max = x;
1624                 }
1625         }
1626         dsp->freqcount = max;
1627 }
1628
1629 struct ast_dsp *ast_dsp_new(void)
1630 {
1631         struct ast_dsp *dsp;
1632         dsp = malloc(sizeof(struct ast_dsp));
1633         if (dsp) {
1634                 memset(dsp, 0, sizeof(struct ast_dsp));
1635                 dsp->threshold = DEFAULT_THRESHOLD;
1636                 dsp->features = DSP_FEATURE_SILENCE_SUPPRESS;
1637                 dsp->busycount = DSP_HISTORY;
1638                 /* Initialize DTMF detector */
1639                 ast_dtmf_detect_init(&dsp->td.dtmf);
1640                 /* Initialize initial DSP progress detect parameters */
1641                 ast_dsp_prog_reset(dsp);
1642         }
1643         return dsp;
1644 }
1645
1646 void ast_dsp_set_features(struct ast_dsp *dsp, int features)
1647 {
1648         dsp->features = features;
1649 }
1650
1651 void ast_dsp_free(struct ast_dsp *dsp)
1652 {
1653         free(dsp);
1654 }
1655
1656 void ast_dsp_set_threshold(struct ast_dsp *dsp, int threshold)
1657 {
1658         dsp->threshold = threshold;
1659 }
1660
1661 void ast_dsp_set_busy_count(struct ast_dsp *dsp, int cadences)
1662 {
1663         if (cadences < 4)
1664                 cadences = 4;
1665         if (cadences > DSP_HISTORY)
1666                 cadences = DSP_HISTORY;
1667         dsp->busycount = cadences;
1668 }
1669
1670 void ast_dsp_digitreset(struct ast_dsp *dsp)
1671 {
1672         int i;
1673         dsp->thinkdigit = 0;
1674         if (dsp->digitmode & DSP_DIGITMODE_MF) {
1675                 memset(dsp->td.mf.digits, 0, sizeof(dsp->td.mf.digits));
1676                 dsp->td.mf.current_digits = 0;
1677                 /* Reinitialise the detector for the next block */
1678                 for (i = 0;  i < 6;  i++) {
1679                 goertzel_reset(&dsp->td.mf.tone_out[i]);
1680 #ifdef OLD_DSP_ROUTINES
1681                     goertzel_reset(&dsp->td.mf.tone_out2nd[i]);
1682 #endif                  
1683                 }
1684 #ifdef OLD_DSP_ROUTINES
1685                 dsp->td.mf.energy = 0.0;
1686             dsp->td.mf.hit1 = dsp->td.mf.hit2 = dsp->td.mf.hit3 = dsp->td.mf.hit4 = dsp->td.mf.mhit = 0;
1687 #else
1688             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;
1689 #endif          
1690                 dsp->td.mf.current_sample = 0;
1691         } else {
1692                 memset(dsp->td.dtmf.digits, 0, sizeof(dsp->td.dtmf.digits));
1693                 dsp->td.dtmf.current_digits = 0;
1694                 /* Reinitialise the detector for the next block */
1695                 for (i = 0;  i < 4;  i++) {
1696                 goertzel_reset(&dsp->td.dtmf.row_out[i]);
1697                     goertzel_reset(&dsp->td.dtmf.col_out[i]);
1698 #ifdef OLD_DSP_ROUTINES
1699                 goertzel_reset(&dsp->td.dtmf.row_out2nd[i]);
1700                 goertzel_reset(&dsp->td.dtmf.col_out2nd[i]);
1701 #endif                  
1702                 }
1703 #ifdef FAX_DETECT
1704             goertzel_reset (&dsp->td.dtmf.fax_tone);
1705 #endif
1706 #ifdef OLD_DSP_ROUTINES
1707 #ifdef FAX_DETECT
1708             goertzel_reset (&dsp->td.dtmf.fax_tone2nd);
1709 #endif
1710             dsp->td.dtmf.hit1 = dsp->td.dtmf.hit2 = dsp->td.dtmf.hit3 = dsp->td.dtmf.hit4 = dsp->td.dtmf.mhit = 0;
1711 #else
1712             dsp->td.dtmf.hits[2] = dsp->td.dtmf.hits[1] = dsp->td.dtmf.hits[0] =  dsp->td.dtmf.mhit = 0;
1713 #endif          
1714                 dsp->td.dtmf.energy = 0.0;
1715                 dsp->td.dtmf.current_sample = 0;
1716         }
1717 }
1718
1719 void ast_dsp_reset(struct ast_dsp *dsp)
1720 {
1721         int x;
1722         dsp->totalsilence = 0;
1723         dsp->gsamps = 0;
1724         for (x=0;x<4;x++)
1725                 dsp->freqs[x].v2 = dsp->freqs[x].v3 = 0.0;
1726         memset(dsp->historicsilence, 0, sizeof(dsp->historicsilence));
1727         memset(dsp->historicnoise, 0, sizeof(dsp->historicnoise));
1728         
1729 }
1730
1731 int ast_dsp_digitmode(struct ast_dsp *dsp, int digitmode)
1732 {
1733         int new, old;
1734         old = dsp->digitmode & (DSP_DIGITMODE_DTMF | DSP_DIGITMODE_MF | DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX);
1735         new = digitmode & (DSP_DIGITMODE_DTMF | DSP_DIGITMODE_MF | DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX);
1736         if (old != new) {
1737                 /* Must initialize structures if switching from MF to DTMF or vice-versa */
1738                 if (new & DSP_DIGITMODE_MF)
1739                         ast_mf_detect_init(&dsp->td.mf);
1740                 else
1741                         ast_dtmf_detect_init(&dsp->td.dtmf);
1742         }
1743         dsp->digitmode = digitmode;
1744         return 0;
1745 }
1746
1747 int ast_dsp_set_call_progress_zone(struct ast_dsp *dsp, char *zone)
1748 {
1749         int x;
1750         for (x=0;x<sizeof(aliases) / sizeof(aliases[0]);x++) {
1751                 if (!strcasecmp(aliases[x].name, zone)) {
1752                         dsp->progmode = aliases[x].mode;
1753                         ast_dsp_prog_reset(dsp);
1754                         return 0;
1755                 }
1756         }
1757         return -1;
1758 }