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