Fix off by one (bug #2872, but different solution)
[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         if (!len)
1233                 return 0;
1234         
1235         accum = 0;
1236         for (x=0;x<len; x++) 
1237                 accum += abs(s[x]);
1238         accum /= len;
1239         if (accum < dsp->threshold) {
1240                 dsp->totalsilence += len/8;
1241                 if (dsp->totalnoise) {
1242                         /* Move and save history */
1243                         memmove(dsp->historicnoise + DSP_HISTORY - dsp->busycount, dsp->historicnoise + DSP_HISTORY - dsp->busycount +1, dsp->busycount*sizeof(dsp->historicnoise[0]));
1244                         dsp->historicnoise[DSP_HISTORY - 1] = dsp->totalnoise;
1245 /* we don't want to check for busydetect that frequently */
1246 #if 0
1247                         dsp->busymaybe = 1;
1248 #endif
1249                 }
1250                 dsp->totalnoise = 0;
1251                 res = 1;
1252         } else {
1253                 dsp->totalnoise += len/8;
1254                 if (dsp->totalsilence) {
1255                         int silence1 = dsp->historicsilence[DSP_HISTORY - 1];
1256                         int silence2 = dsp->historicsilence[DSP_HISTORY - 2];
1257                         /* Move and save history */
1258                         memmove(dsp->historicsilence + DSP_HISTORY - dsp->busycount, dsp->historicsilence + DSP_HISTORY - dsp->busycount + 1, dsp->busycount*sizeof(dsp->historicsilence[0]));
1259                         dsp->historicsilence[DSP_HISTORY - 1] = dsp->totalsilence;
1260                         /* check if the previous sample differs only by BUSY_PERCENT from the one before it */
1261                         if (silence1 < silence2) {
1262                                 if (silence1 + silence1/BUSY_PERCENT >= silence2)
1263                                         dsp->busymaybe = 1;
1264                                 else 
1265                                         dsp->busymaybe = 0;
1266                         } else {
1267                                 if (silence1 - silence1/BUSY_PERCENT <= silence2)
1268                                         dsp->busymaybe = 1;
1269                                 else 
1270                                         dsp->busymaybe = 0;
1271                         }
1272                                         
1273                 }
1274                 dsp->totalsilence = 0;
1275         }
1276         if (totalsilence)
1277                 *totalsilence = dsp->totalsilence;
1278         return res;
1279 }
1280 #ifdef BUSYDETECT_MARTIN
1281 int ast_dsp_busydetect(struct ast_dsp *dsp)
1282 {
1283         int res = 0, x;
1284 #ifndef BUSYDETECT_TONEONLY
1285         int avgsilence = 0, hitsilence = 0;
1286 #endif
1287         int avgtone = 0, hittone = 0;
1288         if (!dsp->busymaybe)
1289                 return res;
1290         for (x=DSP_HISTORY - dsp->busycount;x<DSP_HISTORY;x++) {
1291 #ifndef BUSYDETECT_TONEONLY
1292                 avgsilence += dsp->historicsilence[x];
1293 #endif
1294                 avgtone += dsp->historicnoise[x];
1295         }
1296 #ifndef BUSYDETECT_TONEONLY
1297         avgsilence /= dsp->busycount;
1298 #endif
1299         avgtone /= dsp->busycount;
1300         for (x=DSP_HISTORY - dsp->busycount;x<DSP_HISTORY;x++) {
1301 #ifndef BUSYDETECT_TONEONLY
1302                 if (avgsilence > dsp->historicsilence[x]) {
1303                         if (avgsilence - (avgsilence / BUSY_PERCENT) <= dsp->historicsilence[x])
1304                                 hitsilence++;
1305                 } else {
1306                         if (avgsilence + (avgsilence / BUSY_PERCENT) >= dsp->historicsilence[x])
1307                                 hitsilence++;
1308                 }
1309 #endif
1310                 if (avgtone > dsp->historicnoise[x]) {
1311                         if (avgtone - (avgtone / BUSY_PERCENT) <= dsp->historicsilence[x])
1312                                 hittone++;
1313                 } else {
1314                         if (avgtone + (avgtone / BUSY_PERCENT) >= dsp->historicsilence[x])
1315                                 hittone++;
1316                 }
1317         }
1318 #ifndef BUSYDETECT_TONEONLY
1319         if ((hittone >= dsp->busycount - 1) && (hitsilence >= dsp->busycount - 1) && (avgtone >= BUSY_MIN && avgtone <= BUSY_MAX) && (avgsilence >= BUSY_MIN && avgsilence <= BUSY_MAX)) {
1320 #else
1321         if ((hittone >= dsp->busycount - 1) && (avgtone >= BUSY_MIN && avgtone <= BUSY_MAX)) {
1322 #endif
1323 #ifdef BUSYDETECT_COMPARE_TONE_AND_SILENCE
1324 #ifdef BUSYDETECT_TONEONLY
1325 #error You cant use BUSYDETECT_TONEONLY together with BUSYDETECT_COMPARE_TONE_AND_SILENCE
1326 #endif
1327                 if (avgtone > avgsilence) {
1328                         if (avgtone - avgtone/(BUSY_PERCENT*2) <= avgsilence)
1329                                 res = 1;
1330                 } else {
1331                         if (avgtone + avgtone/(BUSY_PERCENT*2) >= avgsilence)
1332                                 res = 1;
1333                 }
1334 #else
1335                 res = 1;
1336 #endif
1337         }
1338 #if 0
1339         if (res)
1340                 ast_log(LOG_NOTICE, "detected busy, avgtone: %d, avgsilence %d\n", avgtone, avgsilence);
1341 #endif
1342         return res;
1343 }
1344 #endif
1345
1346 #ifdef BUSYDETECT
1347 int ast_dsp_busydetect(struct ast_dsp *dsp)
1348 {
1349         int x;
1350         int res = 0;
1351         int max, min;
1352
1353 #if 0
1354         if (dsp->busy_hits > 5);
1355         return 0;
1356 #endif
1357         if (dsp->busymaybe) {
1358 #if 0
1359                 printf("Maybe busy!\n");
1360 #endif          
1361                 dsp->busymaybe = 0;
1362                 min = 9999;
1363                 max = 0;
1364                 for (x=DSP_HISTORY - dsp->busycount;x<DSP_HISTORY;x++) {
1365 #if 0
1366                         printf("Silence: %d, Noise: %d\n", dsp->historicsilence[x], dsp->historicnoise[x]);
1367 #endif                  
1368                         if (dsp->historicsilence[x] < min)
1369                                 min = dsp->historicsilence[x];
1370                         if (dsp->historicnoise[x] < min)
1371                                 min = dsp->historicnoise[x];
1372                         if (dsp->historicsilence[x] > max)
1373                                 max = dsp->historicsilence[x];
1374                         if (dsp->historicnoise[x] > max)
1375                                 max = dsp->historicnoise[x];
1376                 }
1377                 if ((max - min < BUSY_THRESHOLD) && (max < BUSY_MAX) && (min > BUSY_MIN)) {
1378 #if 0
1379                         printf("Busy!\n");
1380 #endif                  
1381                         res = 1;
1382                 }
1383 #if 0
1384                 printf("Min: %d, max: %d\n", min, max);
1385 #endif          
1386         }
1387         return res;
1388 }
1389 #endif
1390
1391 int ast_dsp_silence(struct ast_dsp *dsp, struct ast_frame *f, int *totalsilence)
1392 {
1393         short *s;
1394         int len;
1395         
1396         if (f->frametype != AST_FRAME_VOICE) {
1397                 ast_log(LOG_WARNING, "Can't calculate silence on a non-voice frame\n");
1398                 return 0;
1399         }
1400         if (f->subclass != AST_FORMAT_SLINEAR) {
1401                 ast_log(LOG_WARNING, "Can only calculate silence on signed-linear frames :(\n");
1402                 return 0;
1403         }
1404         s = f->data;
1405         len = f->datalen/2;
1406         return __ast_dsp_silence(dsp, s, len, totalsilence);
1407 }
1408
1409 struct ast_frame *ast_dsp_process(struct ast_channel *chan, struct ast_dsp *dsp, struct ast_frame *af)
1410 {
1411         int silence;
1412         int res;
1413         int digit;
1414         int x;
1415         unsigned short *shortdata;
1416         unsigned char *odata;
1417         int len;
1418         int writeback = 0;
1419
1420 #define FIX_INF(inf) do { \
1421                 if (writeback) { \
1422                         switch(inf->subclass) { \
1423                         case AST_FORMAT_SLINEAR: \
1424                                 break; \
1425                         case AST_FORMAT_ULAW: \
1426                                 for (x=0;x<len;x++) \
1427                                         odata[x] = AST_LIN2MU(shortdata[x]); \
1428                                 break; \
1429                         case AST_FORMAT_ALAW: \
1430                                 for (x=0;x<len;x++) \
1431                                         odata[x] = AST_LIN2A(shortdata[x]); \
1432                                 break; \
1433                         } \
1434                 } \
1435         } while(0) 
1436
1437         if (!af)
1438                 return NULL;
1439         if (af->frametype != AST_FRAME_VOICE)
1440                 return af;
1441         odata = af->data;
1442         len = af->datalen;
1443         /* Make sure we have short data */
1444         switch(af->subclass) {
1445         case AST_FORMAT_SLINEAR:
1446                 shortdata = af->data;
1447                 len = af->datalen / 2;
1448                 break;
1449         case AST_FORMAT_ULAW:
1450                 shortdata = alloca(af->datalen * 2);
1451                 if (!shortdata) {
1452                         ast_log(LOG_WARNING, "Unable to allocate stack space for data: %s\n", strerror(errno));
1453                         return af;
1454                 }
1455                 for (x=0;x<len;x++) 
1456                         shortdata[x] = AST_MULAW(odata[x]);
1457                 break;
1458         case AST_FORMAT_ALAW:
1459                 shortdata = alloca(af->datalen * 2);
1460                 if (!shortdata) {
1461                         ast_log(LOG_WARNING, "Unable to allocate stack space for data: %s\n", strerror(errno));
1462                         return af;
1463                 }
1464                 for (x=0;x<len;x++) 
1465                         shortdata[x] = AST_ALAW(odata[x]);
1466                 break;
1467         default:
1468                 ast_log(LOG_WARNING, "Inband DTMF is not supported on codec %s. Use RFC2833\n", ast_codec2str(af->subclass));
1469                 return af;
1470         }
1471         silence = __ast_dsp_silence(dsp, shortdata, len, NULL);
1472         if ((dsp->features & DSP_FEATURE_SILENCE_SUPPRESS) && silence) {
1473                 memset(&dsp->f, 0, sizeof(dsp->f));
1474                 dsp->f.frametype = AST_FRAME_NULL;
1475                 return &dsp->f;
1476         }
1477         if ((dsp->features & DSP_FEATURE_BUSY_DETECT) && ast_dsp_busydetect(dsp)) {
1478                 chan->_softhangup |= AST_SOFTHANGUP_DEV;
1479                 memset(&dsp->f, 0, sizeof(dsp->f));
1480                 dsp->f.frametype = AST_FRAME_CONTROL;
1481                 dsp->f.subclass = AST_CONTROL_BUSY;
1482                 ast_log(LOG_DEBUG, "Requesting Hangup because the busy tone was detected on channel %s\n", chan->name);
1483                 return &dsp->f;
1484         }
1485         if ((dsp->features & DSP_FEATURE_DTMF_DETECT)) {
1486                 digit = __ast_dsp_digitdetect(dsp, shortdata, len, &writeback);
1487 #if 0
1488                 if (digit)
1489                         printf("Performing digit detection returned %d, digitmode is %d\n", digit, dsp->digitmode);
1490 #endif                  
1491                 if (dsp->digitmode & (DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX)) {
1492                         if (!dsp->thinkdigit) {
1493                                 if (digit) {
1494                                         /* Looks like we might have something.  Request a conference mute for the moment */
1495                                         memset(&dsp->f, 0, sizeof(dsp->f));
1496                                         dsp->f.frametype = AST_FRAME_DTMF;
1497                                         dsp->f.subclass = 'm';
1498                                         dsp->thinkdigit = 'x';
1499                                         FIX_INF(af);
1500                                         if (chan)
1501                                                 ast_queue_frame(chan, af);
1502                                         ast_frfree(af);
1503                                         return &dsp->f;
1504                                 }
1505                         } else {
1506                                 if (digit) {
1507                                         /* Thought we saw one last time.  Pretty sure we really have now */
1508                                         if (dsp->thinkdigit) {
1509                                                 if ((dsp->thinkdigit != 'x') && (dsp->thinkdigit != digit)) {
1510                                                         /* If we found a digit, and we're changing digits, go
1511                                                            ahead and send this one, but DON'T stop confmute because
1512                                                            we're detecting something else, too... */
1513                                                         memset(&dsp->f, 0, sizeof(dsp->f));
1514                                                         dsp->f.frametype = AST_FRAME_DTMF;
1515                                                         dsp->f.subclass = dsp->thinkdigit;
1516                                                         FIX_INF(af);
1517                                                         if (chan)
1518                                                                 ast_queue_frame(chan, af);
1519                                                         ast_frfree(af);
1520                                                 }
1521                                                 dsp->thinkdigit = digit;
1522                                                 return &dsp->f;
1523                                         }
1524                                         dsp->thinkdigit = digit;
1525                                 } else {
1526                                         if (dsp->thinkdigit) {
1527                                                 memset(&dsp->f, 0, sizeof(dsp->f));
1528                                                 if (dsp->thinkdigit != 'x') {
1529                                                         /* If we found a digit, send it now */
1530                                                         dsp->f.frametype = AST_FRAME_DTMF;
1531                                                         dsp->f.subclass = dsp->thinkdigit;
1532                                                         dsp->thinkdigit = 0;
1533                                                 } else {
1534                                                         dsp->f.frametype = AST_FRAME_DTMF;
1535                                                         dsp->f.subclass = 'u';
1536                                                         dsp->thinkdigit = 0;
1537                                                 }
1538                                                 FIX_INF(af);
1539                                                 if (chan)
1540                                                         ast_queue_frame(chan, af);
1541                                                 ast_frfree(af);
1542                                                 return &dsp->f;
1543                                         }
1544                                 }
1545                         }
1546                 } else if (!digit) {
1547                         /* Only check when there is *not* a hit... */
1548                         if (dsp->digitmode & DSP_DIGITMODE_MF) {
1549                                 if (dsp->td.mf.current_digits) {
1550                                         memset(&dsp->f, 0, sizeof(dsp->f));
1551                                         dsp->f.frametype = AST_FRAME_DTMF;
1552                                         dsp->f.subclass = dsp->td.mf.digits[0];
1553                                         memmove(dsp->td.mf.digits, dsp->td.mf.digits + 1, dsp->td.mf.current_digits);
1554                                         dsp->td.mf.current_digits--;
1555                                         FIX_INF(af);
1556                                         if (chan)
1557                                                 ast_queue_frame(chan, af);
1558                                         ast_frfree(af);
1559                                         return &dsp->f;
1560                                 }
1561                         } else {
1562                                 if (dsp->td.dtmf.current_digits) {
1563                                         memset(&dsp->f, 0, sizeof(dsp->f));
1564                                         dsp->f.frametype = AST_FRAME_DTMF;
1565                                         dsp->f.subclass = dsp->td.dtmf.digits[0];
1566                                         memmove(dsp->td.dtmf.digits, dsp->td.dtmf.digits + 1, dsp->td.dtmf.current_digits);
1567                                         dsp->td.dtmf.current_digits--;
1568                                         FIX_INF(af);
1569                                         if (chan)
1570                                                 ast_queue_frame(chan, af);
1571                                         ast_frfree(af);
1572                                         return &dsp->f;
1573                                 }
1574                         }
1575                 }
1576         }
1577         if ((dsp->features & DSP_FEATURE_CALL_PROGRESS)) {
1578                 res = __ast_dsp_call_progress(dsp, shortdata, len);
1579                 memset(&dsp->f, 0, sizeof(dsp->f));
1580                 dsp->f.frametype = AST_FRAME_CONTROL;
1581                 if (res) {
1582                         switch(res) {
1583                         case AST_CONTROL_ANSWER:
1584                         case AST_CONTROL_BUSY:
1585                         case AST_CONTROL_RINGING:
1586                         case AST_CONTROL_CONGESTION:
1587                                 dsp->f.subclass = res;
1588                                 if (chan) 
1589                                         ast_queue_frame(chan, &dsp->f);
1590                                 break;
1591                         default:
1592                                 ast_log(LOG_WARNING, "Don't know how to represent call progress message %d\n", res);
1593                         }
1594                 }
1595         }
1596         FIX_INF(af);
1597         return af;
1598 }
1599
1600 static void ast_dsp_prog_reset(struct ast_dsp *dsp)
1601 {
1602         int max = 0;
1603         int x;
1604         dsp->gsamp_size = modes[dsp->progmode].size;
1605         dsp->gsamps = 0;
1606         for (x=0;x<sizeof(modes[dsp->progmode].freqs) / sizeof(modes[dsp->progmode].freqs[0]);x++) {
1607                 if (modes[dsp->progmode].freqs[x]) {
1608                         goertzel_init(&dsp->freqs[x], (float)modes[dsp->progmode].freqs[x], dsp->gsamp_size);
1609                         max = x + 1;
1610                 }
1611         }
1612         dsp->freqcount = max;
1613 }
1614
1615 struct ast_dsp *ast_dsp_new(void)
1616 {
1617         struct ast_dsp *dsp;
1618         dsp = malloc(sizeof(struct ast_dsp));
1619         if (dsp) {
1620                 memset(dsp, 0, sizeof(struct ast_dsp));
1621                 dsp->threshold = DEFAULT_THRESHOLD;
1622                 dsp->features = DSP_FEATURE_SILENCE_SUPPRESS;
1623                 dsp->busycount = DSP_HISTORY;
1624                 /* Initialize DTMF detector */
1625                 ast_dtmf_detect_init(&dsp->td.dtmf);
1626                 /* Initialize initial DSP progress detect parameters */
1627                 ast_dsp_prog_reset(dsp);
1628         }
1629         return dsp;
1630 }
1631
1632 void ast_dsp_set_features(struct ast_dsp *dsp, int features)
1633 {
1634         dsp->features = features;
1635 }
1636
1637 void ast_dsp_free(struct ast_dsp *dsp)
1638 {
1639         free(dsp);
1640 }
1641
1642 void ast_dsp_set_threshold(struct ast_dsp *dsp, int threshold)
1643 {
1644         dsp->threshold = threshold;
1645 }
1646
1647 void ast_dsp_set_busy_count(struct ast_dsp *dsp, int cadences)
1648 {
1649         if (cadences < 4)
1650                 cadences = 4;
1651         if (cadences > DSP_HISTORY)
1652                 cadences = DSP_HISTORY;
1653         dsp->busycount = cadences;
1654 }
1655
1656 void ast_dsp_digitreset(struct ast_dsp *dsp)
1657 {
1658         int i;
1659         dsp->thinkdigit = 0;
1660         if (dsp->digitmode & DSP_DIGITMODE_MF) {
1661                 memset(dsp->td.mf.digits, 0, sizeof(dsp->td.mf.digits));
1662                 dsp->td.mf.current_digits = 0;
1663                 /* Reinitialise the detector for the next block */
1664                 for (i = 0;  i < 6;  i++) {
1665                 goertzel_reset(&dsp->td.mf.tone_out[i]);
1666 #ifdef OLD_DSP_ROUTINES
1667                     goertzel_reset(&dsp->td.mf.tone_out2nd[i]);
1668 #endif                  
1669                 }
1670 #ifdef OLD_DSP_ROUTINES
1671                 dsp->td.mf.energy = 0.0;
1672             dsp->td.mf.hit1 = dsp->td.mf.hit2 = dsp->td.mf.hit3 = dsp->td.mf.hit4 = dsp->td.mf.mhit = 0;
1673 #else
1674             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;
1675 #endif          
1676                 dsp->td.mf.current_sample = 0;
1677         } else {
1678                 memset(dsp->td.dtmf.digits, 0, sizeof(dsp->td.dtmf.digits));
1679                 dsp->td.dtmf.current_digits = 0;
1680                 /* Reinitialise the detector for the next block */
1681                 for (i = 0;  i < 4;  i++) {
1682                 goertzel_reset(&dsp->td.dtmf.row_out[i]);
1683                     goertzel_reset(&dsp->td.dtmf.col_out[i]);
1684 #ifdef OLD_DSP_ROUTINES
1685                 goertzel_reset(&dsp->td.dtmf.row_out2nd[i]);
1686                 goertzel_reset(&dsp->td.dtmf.col_out2nd[i]);
1687 #endif                  
1688                 }
1689 #ifdef FAX_DETECT
1690             goertzel_reset (&dsp->td.dtmf.fax_tone);
1691 #endif
1692 #ifdef OLD_DSP_ROUTINES
1693 #ifdef FAX_DETECT
1694             goertzel_reset (&dsp->td.dtmf.fax_tone2nd);
1695 #endif
1696             dsp->td.dtmf.hit1 = dsp->td.dtmf.hit2 = dsp->td.dtmf.hit3 = dsp->td.dtmf.hit4 = dsp->td.dtmf.mhit = 0;
1697 #else
1698             dsp->td.dtmf.hits[2] = dsp->td.dtmf.hits[1] = dsp->td.dtmf.hits[0] =  dsp->td.dtmf.mhit = 0;
1699 #endif          
1700                 dsp->td.dtmf.energy = 0.0;
1701                 dsp->td.dtmf.current_sample = 0;
1702         }
1703 }
1704
1705 void ast_dsp_reset(struct ast_dsp *dsp)
1706 {
1707         int x;
1708         dsp->totalsilence = 0;
1709         dsp->gsamps = 0;
1710         for (x=0;x<4;x++)
1711                 dsp->freqs[x].v2 = dsp->freqs[x].v3 = 0.0;
1712         memset(dsp->historicsilence, 0, sizeof(dsp->historicsilence));
1713         memset(dsp->historicnoise, 0, sizeof(dsp->historicnoise));
1714         
1715 }
1716
1717 int ast_dsp_digitmode(struct ast_dsp *dsp, int digitmode)
1718 {
1719         int new, old;
1720         old = dsp->digitmode & (DSP_DIGITMODE_DTMF | DSP_DIGITMODE_MF | DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX);
1721         new = digitmode & (DSP_DIGITMODE_DTMF | DSP_DIGITMODE_MF | DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX);
1722         if (old != new) {
1723                 /* Must initialize structures if switching from MF to DTMF or vice-versa */
1724                 if (new & DSP_DIGITMODE_MF)
1725                         ast_mf_detect_init(&dsp->td.mf);
1726                 else
1727                         ast_dtmf_detect_init(&dsp->td.dtmf);
1728         }
1729         dsp->digitmode = digitmode;
1730         return 0;
1731 }
1732
1733 int ast_dsp_set_call_progress_zone(struct ast_dsp *dsp, char *zone)
1734 {
1735         int x;
1736         for (x=0;x<sizeof(aliases) / sizeof(aliases[0]);x++) {
1737                 if (!strcasecmp(aliases[x].name, zone)) {
1738                         dsp->progmode = aliases[x].mode;
1739                         ast_dsp_prog_reset(dsp);
1740                         return 0;
1741                 }
1742         }
1743         return -1;
1744 }