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