Merge twisted's dsp formatting fixes (bug #3218)
[asterisk/asterisk.git] / dsp.c
1 /*
2  * Asterisk -- A telephony toolkit for Linux.
3  *
4  * Convenience Signal Processing routines
5  * 
6  * Copyright (C) 2002, Digium
7  *
8  * Mark Spencer <markster@linux-support.net>
9  *
10  * This program is free software, distributed under the terms of
11  * the GNU General Public License.
12  *
13  * Goertzel routines are borrowed from Steve Underwood's tremendous work on the
14  * DTMF detector.
15  *
16  */
17
18 /* Some routines from tone_detect.c by Steven Underwood as published under the zapata library */
19 /*
20         tone_detect.c - General telephony tone detection, and specific
21                         detection of DTMF.
22
23         Copyright (C) 2001  Steve Underwood <steveu@coppice.org>
24
25         Despite my general liking of the GPL, I place this code in the
26         public domain for the benefit of all mankind - even the slimy
27         ones who might try to proprietize my work and use it to my
28         detriment.
29 */
30
31 #include <sys/types.h>
32 #include <asterisk/frame.h>
33 #include <asterisk/channel.h>
34 #include <asterisk/channel_pvt.h>
35 #include <asterisk/logger.h>
36 #include <asterisk/dsp.h>
37 #include <asterisk/ulaw.h>
38 #include <asterisk/alaw.h>
39 #include <stdlib.h>
40 #include <unistd.h>
41 #include <string.h>
42 #include <math.h>
43 #include <errno.h>
44 #include <stdio.h>
45
46 /* Number of goertzels for progress detect */
47 #define GSAMP_SIZE_NA 183                       /* North America - 350, 440, 480, 620, 950, 1400, 1800 Hz */
48 #define GSAMP_SIZE_CR 188                       /* Costa Rica, 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("Got whole dsp state: 350: %e, 440: %e, 480: %e, 620: %e, 950: %e, 1400: %e, 1800: %e, Energy: %e\n", 
1027                                 hz_350, hz_440, hz_480, hz_620, hz_950, hz_1400, hz_1800, dsp->genergy);
1028 #endif
1029                         switch(dsp->progmode) {
1030                         case PROG_MODE_NA:
1031                                 if (pair_there(hz[HZ_480], hz[HZ_620], hz[HZ_350], hz[HZ_440], dsp->genergy)) {
1032                                         newstate = DSP_TONE_STATE_BUSY;
1033                                 } else if (pair_there(hz[HZ_440], hz[HZ_480], hz[HZ_350], hz[HZ_620], dsp->genergy)) {
1034                                         newstate = DSP_TONE_STATE_RINGING;
1035                                 } else if (pair_there(hz[HZ_350], hz[HZ_440], hz[HZ_480], hz[HZ_620], dsp->genergy)) {
1036                                         newstate = DSP_TONE_STATE_DIALTONE;
1037                                 } else if (hz[HZ_950] > TONE_MIN_THRESH * TONE_THRESH) {
1038                                         newstate = DSP_TONE_STATE_SPECIAL1;
1039                                 } else if (hz[HZ_1400] > TONE_MIN_THRESH * TONE_THRESH) {
1040                                         if (dsp->tstate == DSP_TONE_STATE_SPECIAL1)
1041                                                 newstate = DSP_TONE_STATE_SPECIAL2;
1042                                 } else if (hz[HZ_1800] > TONE_MIN_THRESH * TONE_THRESH) {
1043                                         if (dsp->tstate == DSP_TONE_STATE_SPECIAL2)
1044                                                 newstate = DSP_TONE_STATE_SPECIAL3;
1045                                 } else if (dsp->genergy > TONE_MIN_THRESH * TONE_THRESH) {
1046                                         newstate = DSP_TONE_STATE_TALKING;
1047                                 } else
1048                                         newstate = DSP_TONE_STATE_SILENCE;
1049                                 break;
1050                         case PROG_MODE_CR:
1051                                 if (hz[HZ_425] > TONE_MIN_THRESH * TONE_THRESH) {
1052                                         newstate = DSP_TONE_STATE_RINGING;
1053                                 } else if (dsp->genergy > TONE_MIN_THRESH * TONE_THRESH) {
1054                                         newstate = DSP_TONE_STATE_TALKING;
1055                                 } else
1056                                         newstate = DSP_TONE_STATE_SILENCE;
1057                                 break;
1058                         default:
1059                                 ast_log(LOG_WARNING, "Can't process in unknown prog mode '%d'\n", dsp->progmode);
1060                         }
1061                         if (newstate == dsp->tstate) {
1062                                 dsp->tcount++;
1063                                 if (dsp->tcount == COUNT_THRESH) {
1064                                         if ((dsp->features & DSP_PROGRESS_BUSY) && 
1065                                             dsp->tstate == DSP_TONE_STATE_BUSY) {
1066                                                 res = AST_CONTROL_BUSY;
1067                                                 dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
1068                                         } else if ((dsp->features & DSP_PROGRESS_TALK) && 
1069                                                    dsp->tstate == DSP_TONE_STATE_TALKING) {
1070                                                 res = AST_CONTROL_ANSWER;
1071                                                 dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
1072                                         } else if ((dsp->features & DSP_PROGRESS_RINGING) && 
1073                                                    dsp->tstate == DSP_TONE_STATE_RINGING)
1074                                                 res = AST_CONTROL_RINGING;
1075                                         else if ((dsp->features & DSP_PROGRESS_CONGESTION) && 
1076                                                  dsp->tstate == DSP_TONE_STATE_SPECIAL3) {
1077                                                 res = AST_CONTROL_CONGESTION;
1078                                                 dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
1079                                         }
1080                                 }
1081                         } else {
1082 #if 0
1083                                 printf("Newstate: %d\n", newstate);
1084 #endif
1085                                 dsp->tstate = newstate;
1086                                 dsp->tcount = 1;
1087                         }
1088                         
1089                         /* Reset goertzel */                                            
1090                         for (x=0;x<7;x++)
1091                                 dsp->freqs[x].v2 = dsp->freqs[x].v3 = 0.0;
1092                         dsp->gsamps = 0;
1093                         dsp->genergy = 0.0;
1094                 }
1095         }
1096 #if 0
1097         if (res)
1098                 printf("Returning %d\n", res);
1099 #endif          
1100         return res;
1101 }
1102
1103 int ast_dsp_call_progress(struct ast_dsp *dsp, struct ast_frame *inf)
1104 {
1105         if (inf->frametype != AST_FRAME_VOICE) {
1106                 ast_log(LOG_WARNING, "Can't check call progress of non-voice frames\n");
1107                 return 0;
1108         }
1109         if (inf->subclass != AST_FORMAT_SLINEAR) {
1110                 ast_log(LOG_WARNING, "Can only check call progress in signed-linear frames\n");
1111                 return 0;
1112         }
1113         return __ast_dsp_call_progress(dsp, inf->data, inf->datalen / 2);
1114 }
1115
1116 static int __ast_dsp_silence(struct ast_dsp *dsp, short *s, int len, int *totalsilence)
1117 {
1118         int accum;
1119         int x;
1120         int res = 0;
1121
1122         if (!len)
1123                 return 0;
1124         accum = 0;
1125         for (x=0;x<len; x++) 
1126                 accum += abs(s[x]);
1127         accum /= len;
1128         if (accum < dsp->threshold) {
1129                 dsp->totalsilence += len/8;
1130                 if (dsp->totalnoise) {
1131                         /* Move and save history */
1132                         memmove(dsp->historicnoise + DSP_HISTORY - dsp->busycount, dsp->historicnoise + DSP_HISTORY - dsp->busycount +1, dsp->busycount*sizeof(dsp->historicnoise[0]));
1133                         dsp->historicnoise[DSP_HISTORY - 1] = dsp->totalnoise;
1134 /* we don't want to check for busydetect that frequently */
1135 #if 0
1136                         dsp->busymaybe = 1;
1137 #endif
1138                 }
1139                 dsp->totalnoise = 0;
1140                 res = 1;
1141         } else {
1142                 dsp->totalnoise += len/8;
1143                 if (dsp->totalsilence) {
1144                         int silence1 = dsp->historicsilence[DSP_HISTORY - 1];
1145                         int silence2 = dsp->historicsilence[DSP_HISTORY - 2];
1146                         /* Move and save history */
1147                         memmove(dsp->historicsilence + DSP_HISTORY - dsp->busycount, dsp->historicsilence + DSP_HISTORY - dsp->busycount + 1, dsp->busycount*sizeof(dsp->historicsilence[0]));
1148                         dsp->historicsilence[DSP_HISTORY - 1] = dsp->totalsilence;
1149                         /* check if the previous sample differs only by BUSY_PERCENT from the one before it */
1150                         if (silence1 < silence2) {
1151                                 if (silence1 + silence1/BUSY_PERCENT >= silence2)
1152                                         dsp->busymaybe = 1;
1153                                 else 
1154                                         dsp->busymaybe = 0;
1155                         } else {
1156                                 if (silence1 - silence1/BUSY_PERCENT <= silence2)
1157                                         dsp->busymaybe = 1;
1158                                 else 
1159                                         dsp->busymaybe = 0;
1160                         }
1161                 }
1162                 dsp->totalsilence = 0;
1163         }
1164         if (totalsilence)
1165                 *totalsilence = dsp->totalsilence;
1166         return res;
1167 }
1168 #ifdef BUSYDETECT_MARTIN
1169 int ast_dsp_busydetect(struct ast_dsp *dsp)
1170 {
1171         int res = 0, x;
1172 #ifndef BUSYDETECT_TONEONLY
1173         int avgsilence = 0, hitsilence = 0;
1174 #endif
1175         int avgtone = 0, hittone = 0;
1176         if (!dsp->busymaybe)
1177                 return res;
1178         for (x=DSP_HISTORY - dsp->busycount;x<DSP_HISTORY;x++) {
1179 #ifndef BUSYDETECT_TONEONLY
1180                 avgsilence += dsp->historicsilence[x];
1181 #endif
1182                 avgtone += dsp->historicnoise[x];
1183         }
1184 #ifndef BUSYDETECT_TONEONLY
1185         avgsilence /= dsp->busycount;
1186 #endif
1187         avgtone /= dsp->busycount;
1188         for (x=DSP_HISTORY - dsp->busycount;x<DSP_HISTORY;x++) {
1189 #ifndef BUSYDETECT_TONEONLY
1190                 if (avgsilence > dsp->historicsilence[x]) {
1191                         if (avgsilence - (avgsilence / BUSY_PERCENT) <= dsp->historicsilence[x])
1192                                 hitsilence++;
1193                 } else {
1194                         if (avgsilence + (avgsilence / BUSY_PERCENT) >= dsp->historicsilence[x])
1195                                 hitsilence++;
1196                 }
1197 #endif
1198                 if (avgtone > dsp->historicnoise[x]) {
1199                         if (avgtone - (avgtone / BUSY_PERCENT) <= dsp->historicsilence[x])
1200                                 hittone++;
1201                 } else {
1202                         if (avgtone + (avgtone / BUSY_PERCENT) >= dsp->historicsilence[x])
1203                                 hittone++;
1204                 }
1205         }
1206 #ifndef BUSYDETECT_TONEONLY
1207         if ((hittone >= dsp->busycount - 1) && (hitsilence >= dsp->busycount - 1) && 
1208             (avgtone >= BUSY_MIN && avgtone <= BUSY_MAX) && 
1209             (avgsilence >= BUSY_MIN && avgsilence <= BUSY_MAX)) {
1210 #else
1211         if ((hittone >= dsp->busycount - 1) && (avgtone >= BUSY_MIN && avgtone <= BUSY_MAX)) {
1212 #endif
1213 #ifdef BUSYDETECT_COMPARE_TONE_AND_SILENCE
1214 #ifdef BUSYDETECT_TONEONLY
1215 #error You cant use BUSYDETECT_TONEONLY together with BUSYDETECT_COMPARE_TONE_AND_SILENCE
1216 #endif
1217                 if (avgtone > avgsilence) {
1218                         if (avgtone - avgtone/(BUSY_PERCENT*2) <= avgsilence)
1219                                 res = 1;
1220                 } else {
1221                         if (avgtone + avgtone/(BUSY_PERCENT*2) >= avgsilence)
1222                                 res = 1;
1223                 }
1224 #else
1225                 res = 1;
1226 #endif
1227         }
1228 #if 0
1229         if (res)
1230                 ast_log(LOG_NOTICE, "detected busy, avgtone: %d, avgsilence %d\n", avgtone, avgsilence);
1231 #endif
1232         return res;
1233 }
1234 #endif
1235
1236 #ifdef BUSYDETECT
1237 int ast_dsp_busydetect(struct ast_dsp *dsp)
1238 {
1239         int x;
1240         int res = 0;
1241         int max, min;
1242
1243 #if 0
1244         if (dsp->busy_hits > 5);
1245         return 0;
1246 #endif
1247         if (dsp->busymaybe) {
1248 #if 0
1249                 printf("Maybe busy!\n");
1250 #endif          
1251                 dsp->busymaybe = 0;
1252                 min = 9999;
1253                 max = 0;
1254                 for (x=DSP_HISTORY - dsp->busycount;x<DSP_HISTORY;x++) {
1255 #if 0
1256                         printf("Silence: %d, Noise: %d\n", dsp->historicsilence[x], dsp->historicnoise[x]);
1257 #endif                  
1258                         if (dsp->historicsilence[x] < min)
1259                                 min = dsp->historicsilence[x];
1260                         if (dsp->historicnoise[x] < min)
1261                                 min = dsp->historicnoise[x];
1262                         if (dsp->historicsilence[x] > max)
1263                                 max = dsp->historicsilence[x];
1264                         if (dsp->historicnoise[x] > max)
1265                                 max = dsp->historicnoise[x];
1266                 }
1267                 if ((max - min < BUSY_THRESHOLD) && (max < BUSY_MAX) && (min > BUSY_MIN)) {
1268 #if 0
1269                         printf("Busy!\n");
1270 #endif                  
1271                         res = 1;
1272                 }
1273 #if 0
1274                 printf("Min: %d, max: %d\n", min, max);
1275 #endif          
1276         }
1277         return res;
1278 }
1279 #endif
1280
1281 int ast_dsp_silence(struct ast_dsp *dsp, struct ast_frame *f, int *totalsilence)
1282 {
1283         short *s;
1284         int len;
1285         
1286         if (f->frametype != AST_FRAME_VOICE) {
1287                 ast_log(LOG_WARNING, "Can't calculate silence on a non-voice frame\n");
1288                 return 0;
1289         }
1290         if (f->subclass != AST_FORMAT_SLINEAR) {
1291                 ast_log(LOG_WARNING, "Can only calculate silence on signed-linear frames :(\n");
1292                 return 0;
1293         }
1294         s = f->data;
1295         len = f->datalen/2;
1296         return __ast_dsp_silence(dsp, s, len, totalsilence);
1297 }
1298
1299 struct ast_frame *ast_dsp_process(struct ast_channel *chan, struct ast_dsp *dsp, struct ast_frame *af)
1300 {
1301         int silence;
1302         int res;
1303         int digit;
1304         int x;
1305         unsigned short *shortdata;
1306         unsigned char *odata;
1307         int len;
1308         int writeback = 0;
1309
1310 #define FIX_INF(inf) do { \
1311                 if (writeback) { \
1312                         switch(inf->subclass) { \
1313                         case AST_FORMAT_SLINEAR: \
1314                                 break; \
1315                         case AST_FORMAT_ULAW: \
1316                                 for (x=0;x<len;x++) \
1317                                         odata[x] = AST_LIN2MU(shortdata[x]); \
1318                                 break; \
1319                         case AST_FORMAT_ALAW: \
1320                                 for (x=0;x<len;x++) \
1321                                         odata[x] = AST_LIN2A(shortdata[x]); \
1322                                 break; \
1323                         } \
1324                 } \
1325         } while(0) 
1326
1327         if (!af)
1328                 return NULL;
1329         if (af->frametype != AST_FRAME_VOICE)
1330                 return af;
1331         odata = af->data;
1332         len = af->datalen;
1333         /* Make sure we have short data */
1334         switch(af->subclass) {
1335         case AST_FORMAT_SLINEAR:
1336                 shortdata = af->data;
1337                 len = af->datalen / 2;
1338                 break;
1339         case AST_FORMAT_ULAW:
1340                 shortdata = alloca(af->datalen * 2);
1341                 if (!shortdata) {
1342                         ast_log(LOG_WARNING, "Unable to allocate stack space for data: %s\n", strerror(errno));
1343                         return af;
1344                 }
1345                 for (x=0;x<len;x++) 
1346                         shortdata[x] = AST_MULAW(odata[x]);
1347                 break;
1348         case AST_FORMAT_ALAW:
1349                 shortdata = alloca(af->datalen * 2);
1350                 if (!shortdata) {
1351                         ast_log(LOG_WARNING, "Unable to allocate stack space for data: %s\n", strerror(errno));
1352                         return af;
1353                 }
1354                 for (x=0;x<len;x++) 
1355                         shortdata[x] = AST_ALAW(odata[x]);
1356                 break;
1357         default:
1358                 ast_log(LOG_WARNING, "Inband DTMF is not supported on codec %s. Use RFC2833\n", ast_getformatname(af->subclass));
1359                 return af;
1360         }
1361         silence = __ast_dsp_silence(dsp, shortdata, len, NULL);
1362         if ((dsp->features & DSP_FEATURE_SILENCE_SUPPRESS) && silence) {
1363                 memset(&dsp->f, 0, sizeof(dsp->f));
1364                 dsp->f.frametype = AST_FRAME_NULL;
1365                 return &dsp->f;
1366         }
1367         if ((dsp->features & DSP_FEATURE_BUSY_DETECT) && ast_dsp_busydetect(dsp)) {
1368                 chan->_softhangup |= AST_SOFTHANGUP_DEV;
1369                 memset(&dsp->f, 0, sizeof(dsp->f));
1370                 dsp->f.frametype = AST_FRAME_CONTROL;
1371                 dsp->f.subclass = AST_CONTROL_BUSY;
1372                 ast_log(LOG_DEBUG, "Requesting Hangup because the busy tone was detected on channel %s\n", chan->name);
1373                 return &dsp->f;
1374         }
1375         if ((dsp->features & DSP_FEATURE_DTMF_DETECT)) {
1376                 digit = __ast_dsp_digitdetect(dsp, shortdata, len, &writeback);
1377 #if 0
1378                 if (digit)
1379                         printf("Performing digit detection returned %d, digitmode is %d\n", digit, dsp->digitmode);
1380 #endif                  
1381                 if (dsp->digitmode & (DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX)) {
1382                         if (!dsp->thinkdigit) {
1383                                 if (digit) {
1384                                         /* Looks like we might have something.  
1385                                          * Request a conference mute for the moment */
1386                                         memset(&dsp->f, 0, sizeof(dsp->f));
1387                                         dsp->f.frametype = AST_FRAME_DTMF;
1388                                         dsp->f.subclass = 'm';
1389                                         dsp->thinkdigit = 'x';
1390                                         FIX_INF(af);
1391                                         if (chan)
1392                                                 ast_queue_frame(chan, af);
1393                                         ast_frfree(af);
1394                                         return &dsp->f;
1395                                 }
1396                         } else {
1397                                 if (digit) {
1398                                         /* Thought we saw one last time.  Pretty sure we really have now */
1399                                         if (dsp->thinkdigit) {
1400                                                 if ((dsp->thinkdigit != 'x') && (dsp->thinkdigit != digit)) {
1401                                                         /* If we found a digit, and we're changing digits, go
1402                                                            ahead and send this one, but DON'T stop confmute because
1403                                                            we're detecting something else, too... */
1404                                                         memset(&dsp->f, 0, sizeof(dsp->f));
1405                                                         dsp->f.frametype = AST_FRAME_DTMF;
1406                                                         dsp->f.subclass = dsp->thinkdigit;
1407                                                         FIX_INF(af);
1408                                                         if (chan)
1409                                                                 ast_queue_frame(chan, af);
1410                                                         ast_frfree(af);
1411                                                 }
1412                                                 dsp->thinkdigit = digit;
1413                                                 return &dsp->f;
1414                                         }
1415                                         dsp->thinkdigit = digit;
1416                                 } else {
1417                                         if (dsp->thinkdigit) {
1418                                                 memset(&dsp->f, 0, sizeof(dsp->f));
1419                                                 if (dsp->thinkdigit != 'x') {
1420                                                         /* If we found a digit, send it now */
1421                                                         dsp->f.frametype = AST_FRAME_DTMF;
1422                                                         dsp->f.subclass = dsp->thinkdigit;
1423                                                         dsp->thinkdigit = 0;
1424                                                 } else {
1425                                                         dsp->f.frametype = AST_FRAME_DTMF;
1426                                                         dsp->f.subclass = 'u';
1427                                                         dsp->thinkdigit = 0;
1428                                                 }
1429                                                 FIX_INF(af);
1430                                                 if (chan)
1431                                                         ast_queue_frame(chan, af);
1432                                                 ast_frfree(af);
1433                                                 return &dsp->f;
1434                                         }
1435                                 }
1436                         }
1437                 } else if (!digit) {
1438                         /* Only check when there is *not* a hit... */
1439                         if (dsp->digitmode & DSP_DIGITMODE_MF) {
1440                                 if (dsp->td.mf.current_digits) {
1441                                         memset(&dsp->f, 0, sizeof(dsp->f));
1442                                         dsp->f.frametype = AST_FRAME_DTMF;
1443                                         dsp->f.subclass = dsp->td.mf.digits[0];
1444                                         memmove(dsp->td.mf.digits, dsp->td.mf.digits + 1, dsp->td.mf.current_digits);
1445                                         dsp->td.mf.current_digits--;
1446                                         FIX_INF(af);
1447                                         if (chan)
1448                                                 ast_queue_frame(chan, af);
1449                                         ast_frfree(af);
1450                                         return &dsp->f;
1451                                 }
1452                         } else {
1453                                 if (dsp->td.dtmf.current_digits) {
1454                                         memset(&dsp->f, 0, sizeof(dsp->f));
1455                                         dsp->f.frametype = AST_FRAME_DTMF;
1456                                         dsp->f.subclass = dsp->td.dtmf.digits[0];
1457                                         memmove(dsp->td.dtmf.digits, dsp->td.dtmf.digits + 1, dsp->td.dtmf.current_digits);
1458                                         dsp->td.dtmf.current_digits--;
1459                                         FIX_INF(af);
1460                                         if (chan)
1461                                                 ast_queue_frame(chan, af);
1462                                         ast_frfree(af);
1463                                         return &dsp->f;
1464                                 }
1465                         }
1466                 }
1467         }
1468         if ((dsp->features & DSP_FEATURE_CALL_PROGRESS)) {
1469                 res = __ast_dsp_call_progress(dsp, shortdata, len);
1470                 if (res) {
1471                         switch(res) {
1472                         case AST_CONTROL_ANSWER:
1473                         case AST_CONTROL_BUSY:
1474                         case AST_CONTROL_RINGING:
1475                         case AST_CONTROL_CONGESTION:
1476                                 memset(&dsp->f, 0, sizeof(dsp->f));
1477                                 dsp->f.frametype = AST_FRAME_CONTROL;
1478                                 dsp->f.subclass = res;
1479                                 dsp->f.src = "dsp_progress";
1480                                 if (chan) 
1481                                         ast_queue_frame(chan, &dsp->f);
1482                                 break;
1483                         default:
1484                                 ast_log(LOG_WARNING, "Don't know how to represent call progress message %d\n", res);
1485                         }
1486                 }
1487         }
1488         FIX_INF(af);
1489         return af;
1490 }
1491
1492 static void ast_dsp_prog_reset(struct ast_dsp *dsp)
1493 {
1494         int max = 0;
1495         int x;
1496         
1497         dsp->gsamp_size = modes[dsp->progmode].size;
1498         dsp->gsamps = 0;
1499         for (x=0;x<sizeof(modes[dsp->progmode].freqs) / sizeof(modes[dsp->progmode].freqs[0]);x++) {
1500                 if (modes[dsp->progmode].freqs[x]) {
1501                         goertzel_init(&dsp->freqs[x], (float)modes[dsp->progmode].freqs[x], dsp->gsamp_size);
1502                         max = x + 1;
1503                 }
1504         }
1505         dsp->freqcount = max;
1506 }
1507
1508 struct ast_dsp *ast_dsp_new(void)
1509 {
1510         struct ast_dsp *dsp;
1511
1512         dsp = malloc(sizeof(struct ast_dsp));
1513         if (dsp) {
1514                 memset(dsp, 0, sizeof(struct ast_dsp));
1515                 dsp->threshold = DEFAULT_THRESHOLD;
1516                 dsp->features = DSP_FEATURE_SILENCE_SUPPRESS;
1517                 dsp->busycount = DSP_HISTORY;
1518                 /* Initialize DTMF detector */
1519                 ast_dtmf_detect_init(&dsp->td.dtmf);
1520                 /* Initialize initial DSP progress detect parameters */
1521                 ast_dsp_prog_reset(dsp);
1522         }
1523         return dsp;
1524 }
1525
1526 void ast_dsp_set_features(struct ast_dsp *dsp, int features)
1527 {
1528         dsp->features = features;
1529 }
1530
1531 void ast_dsp_free(struct ast_dsp *dsp)
1532 {
1533         free(dsp);
1534 }
1535
1536 void ast_dsp_set_threshold(struct ast_dsp *dsp, int threshold)
1537 {
1538         dsp->threshold = threshold;
1539 }
1540
1541 void ast_dsp_set_busy_count(struct ast_dsp *dsp, int cadences)
1542 {
1543         if (cadences < 4)
1544                 cadences = 4;
1545         if (cadences > DSP_HISTORY)
1546                 cadences = DSP_HISTORY;
1547         dsp->busycount = cadences;
1548 }
1549
1550 void ast_dsp_digitreset(struct ast_dsp *dsp)
1551 {
1552         int i;
1553         
1554         dsp->thinkdigit = 0;
1555         if (dsp->digitmode & DSP_DIGITMODE_MF) {
1556                 memset(dsp->td.mf.digits, 0, sizeof(dsp->td.mf.digits));
1557                 dsp->td.mf.current_digits = 0;
1558                 /* Reinitialise the detector for the next block */
1559                 for (i = 0;  i < 6;  i++) {
1560                         goertzel_reset(&dsp->td.mf.tone_out[i]);
1561 #ifdef OLD_DSP_ROUTINES
1562                         goertzel_reset(&dsp->td.mf.tone_out2nd[i]);
1563 #endif                  
1564                 }
1565 #ifdef OLD_DSP_ROUTINES
1566                 dsp->td.mf.energy = 0.0;
1567                 dsp->td.mf.hit1 = dsp->td.mf.hit2 = dsp->td.mf.hit3 = dsp->td.mf.hit4 = dsp->td.mf.mhit = 0;
1568 #else
1569                 dsp->td.mf.hits[4] = dsp->td.mf.hits[3] = dsp->td.mf.hits[2] = dsp->td.mf.hits[1] = dsp->td.mf.hits[0] = dsp->td.mf.mhit = 0;
1570 #endif          
1571                 dsp->td.mf.current_sample = 0;
1572         } else {
1573                 memset(dsp->td.dtmf.digits, 0, sizeof(dsp->td.dtmf.digits));
1574                 dsp->td.dtmf.current_digits = 0;
1575                 /* Reinitialise the detector for the next block */
1576                 for (i = 0;  i < 4;  i++) {
1577                         goertzel_reset(&dsp->td.dtmf.row_out[i]);
1578                         goertzel_reset(&dsp->td.dtmf.col_out[i]);
1579 #ifdef OLD_DSP_ROUTINES
1580                         goertzel_reset(&dsp->td.dtmf.row_out2nd[i]);
1581                         goertzel_reset(&dsp->td.dtmf.col_out2nd[i]);
1582 #endif                  
1583                 }
1584 #ifdef FAX_DETECT
1585                 goertzel_reset (&dsp->td.dtmf.fax_tone);
1586 #endif
1587 #ifdef OLD_DSP_ROUTINES
1588 #ifdef FAX_DETECT
1589                 goertzel_reset (&dsp->td.dtmf.fax_tone2nd);
1590 #endif
1591                 dsp->td.dtmf.hit1 = dsp->td.dtmf.hit2 = dsp->td.dtmf.hit3 = dsp->td.dtmf.hit4 = dsp->td.dtmf.mhit = 0;
1592 #else
1593                 dsp->td.dtmf.hits[2] = dsp->td.dtmf.hits[1] = dsp->td.dtmf.hits[0] =  dsp->td.dtmf.mhit = 0;
1594 #endif          
1595                 dsp->td.dtmf.energy = 0.0;
1596                 dsp->td.dtmf.current_sample = 0;
1597         }
1598 }
1599
1600 void ast_dsp_reset(struct ast_dsp *dsp)
1601 {
1602         int x;
1603         
1604         dsp->totalsilence = 0;
1605         dsp->gsamps = 0;
1606         for (x=0;x<4;x++)
1607                 dsp->freqs[x].v2 = dsp->freqs[x].v3 = 0.0;
1608         memset(dsp->historicsilence, 0, sizeof(dsp->historicsilence));
1609         memset(dsp->historicnoise, 0, sizeof(dsp->historicnoise));      
1610 }
1611
1612 int ast_dsp_digitmode(struct ast_dsp *dsp, int digitmode)
1613 {
1614         int new;
1615         int old;
1616         
1617         old = dsp->digitmode & (DSP_DIGITMODE_DTMF | DSP_DIGITMODE_MF | DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX);
1618         new = digitmode & (DSP_DIGITMODE_DTMF | DSP_DIGITMODE_MF | DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX);
1619         if (old != new) {
1620                 /* Must initialize structures if switching from MF to DTMF or vice-versa */
1621                 if (new & DSP_DIGITMODE_MF)
1622                         ast_mf_detect_init(&dsp->td.mf);
1623                 else
1624                         ast_dtmf_detect_init(&dsp->td.dtmf);
1625         }
1626         dsp->digitmode = digitmode;
1627         return 0;
1628 }
1629
1630 int ast_dsp_set_call_progress_zone(struct ast_dsp *dsp, char *zone)
1631 {
1632         int x;
1633         
1634         for (x=0;x<sizeof(aliases) / sizeof(aliases[0]);x++) {
1635                 if (!strcasecmp(aliases[x].name, zone)) {
1636                         dsp->progmode = aliases[x].mode;
1637                         ast_dsp_prog_reset(dsp);
1638                         return 0;
1639                 }
1640         }
1641         return -1;
1642 }
1643
1644 int ast_dsp_get_tstate(struct ast_dsp *dsp) 
1645 {
1646         return dsp->tstate;
1647 }
1648
1649 int ast_dsp_get_tcount(struct ast_dsp *dsp) 
1650 {
1651         return dsp->tcount;
1652 }