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