Large cleanup of DSP code
[asterisk/asterisk.git] / main / dsp.c
1 /*
2  * Asterisk -- An open source telephony toolkit.
3  *
4  * Copyright (C) 1999 - 2005, Digium, Inc.
5  *
6  * Mark Spencer <markster@digium.com>
7  *
8  * Goertzel routines are borrowed from Steve Underwood's tremendous work on the
9  * DTMF detector.
10  *
11  * See http://www.asterisk.org for more information about
12  * the Asterisk project. Please do not directly contact
13  * any of the maintainers of this project for assistance;
14  * the project provides a web site, mailing lists and IRC
15  * channels for your use.
16  *
17  * This program is free software, distributed under the terms of
18  * the GNU General Public License Version 2. See the LICENSE file
19  * at the top of the source tree.
20  */
21
22 /*! \file
23  *
24  * \brief Convenience Signal Processing routines
25  *
26  * \author Mark Spencer <markster@digium.com>
27  * \author Steve Underwood <steveu@coppice.org>
28  */
29
30 /* Some routines from tone_detect.c by Steven Underwood as published under the zapata library */
31 /*
32         tone_detect.c - General telephony tone detection, and specific
33                                         detection of DTMF.
34
35         Copyright (C) 2001  Steve Underwood <steveu@coppice.org>
36
37         Despite my general liking of the GPL, I place this code in the
38         public domain for the benefit of all mankind - even the slimy
39         ones who might try to proprietize my work and use it to my
40         detriment.
41 */
42
43 #include "asterisk.h"
44
45 ASTERISK_FILE_VERSION(__FILE__, "$Revision$")
46
47 #include <math.h>
48
49 #include "asterisk/frame.h"
50 #include "asterisk/channel.h"
51 #include "asterisk/dsp.h"
52 #include "asterisk/ulaw.h"
53 #include "asterisk/alaw.h"
54 #include "asterisk/utils.h"
55 #include "asterisk/options.h"
56 #include "asterisk/config.h"
57
58 /*! Number of goertzels for progress detect */
59 enum gsamp_size {
60         GSAMP_SIZE_NA = 183,                    /*!< North America - 350, 440, 480, 620, 950, 1400, 1800 Hz */
61         GSAMP_SIZE_CR = 188,                    /*!< Costa Rica, Brazil - Only care about 425 Hz */
62         GSAMP_SIZE_UK = 160                     /*!< UK disconnect goertzel feed - should trigger 400hz */
63 };
64
65 enum prog_mode {
66         PROG_MODE_NA = 0,
67         PROG_MODE_CR,
68         PROG_MODE_UK
69 };
70
71 enum freq_index { 
72         /*! For US modes { */
73         HZ_350 = 0,
74         HZ_440,
75         HZ_480,
76         HZ_620,
77         HZ_950,
78         HZ_1400,
79         HZ_1800, /*!< } */
80
81         /*! For CR/BR modes */
82         HZ_425 = 0,
83
84         /*! For UK mode */
85         HZ_400 = 0
86 };
87
88 static struct progalias {
89         char *name;
90         enum prog_mode mode;
91 } aliases[] = {
92         { "us", PROG_MODE_NA },
93         { "ca", PROG_MODE_NA },
94         { "cr", PROG_MODE_CR },
95         { "br", PROG_MODE_CR },
96         { "uk", PROG_MODE_UK },
97 };
98
99 static struct progress {
100         enum gsamp_size size;
101         int freqs[7];
102 } modes[] = {
103         { GSAMP_SIZE_NA, { 350, 440, 480, 620, 950, 1400, 1800 } },     /*!< North America */
104         { GSAMP_SIZE_CR, { 425 } },                                     /*!< Costa Rica, Brazil */
105         { GSAMP_SIZE_UK, { 400 } },                                     /*!< UK */
106 };
107
108 #define DEFAULT_THRESHOLD       512
109
110 enum busy_detect {
111         BUSY_PERCENT = 10,      /*!< The percentage difference between the two last silence periods */
112         BUSY_PAT_PERCENT = 7,   /*!< The percentage difference between measured and actual pattern */
113         BUSY_THRESHOLD = 100,   /*!< Max number of ms difference between max and min times in busy */
114         BUSY_MIN = 75,          /*!< Busy must be at least 80 ms in half-cadence */
115         BUSY_MAX =3100          /*!< Busy can't be longer than 3100 ms in half-cadence */
116 };
117
118 /*! Remember last 15 units */
119 #define DSP_HISTORY             15
120
121 #define TONE_THRESH             10.0    /*!< How much louder the tone should be than channel energy */
122 #define TONE_MIN_THRESH         1e8     /*!< How much tone there should be at least to attempt */
123
124 /*! All THRESH_XXX values are in GSAMP_SIZE chunks (us = 22ms) */
125 enum gsamp_thresh {
126         THRESH_RING = 8,                /*!< Need at least 150ms ring to accept */
127         THRESH_TALK = 2,                /*!< Talk detection does not work continuously */
128         THRESH_BUSY = 4,                /*!< Need at least 80ms to accept */
129         THRESH_CONGESTION = 4,          /*!< Need at least 80ms to accept */
130         THRESH_HANGUP = 60,             /*!< Need at least 1300ms to accept hangup */
131         THRESH_RING2ANSWER = 300        /*!< Timeout from start of ring to answer (about 6600 ms) */
132 };
133
134 #define MAX_DTMF_DIGITS         128
135
136 /* Basic DTMF specs:
137  *
138  * Minimum tone on = 40ms
139  * Minimum tone off = 50ms
140  * Maximum digit rate = 10 per second
141  * Normal twist <= 8dB accepted
142  * Reverse twist <= 4dB accepted
143  * S/N >= 15dB will detect OK
144  * Attenuation <= 26dB will detect OK
145  * Frequency tolerance +- 1.5% will detect, +-3.5% will reject
146  */
147
148 #define DTMF_THRESHOLD          8.0e7
149 #define FAX_THRESHOLD           8.0e7
150 #define FAX_2ND_HARMONIC        2.0     /* 4dB */
151 #define DTMF_NORMAL_TWIST       6.3     /* 8dB */
152 #ifdef  RADIO_RELAX
153 #define DTMF_REVERSE_TWIST          (relax ? 6.5 : 2.5)     /* 4dB normal */
154 #else
155 #define DTMF_REVERSE_TWIST          (relax ? 4.0 : 2.5)     /* 4dB normal */
156 #endif
157 #define DTMF_RELATIVE_PEAK_ROW  6.3     /* 8dB */
158 #define DTMF_RELATIVE_PEAK_COL  6.3     /* 8dB */
159 #define DTMF_2ND_HARMONIC_ROW       (relax ? 1.7 : 2.5)     /* 4dB normal */
160 #define DTMF_2ND_HARMONIC_COL   63.1    /* 18dB */
161 #define DTMF_TO_TOTAL_ENERGY    42.0
162
163 #define BELL_MF_THRESHOLD       1.6e9
164 #define BELL_MF_TWIST           4.0     /* 6dB */
165 #define BELL_MF_RELATIVE_PEAK   12.6    /* 11dB */
166
167 #if defined(BUSYDETECT_TONEONLY) && defined(BUSYDETECT_COMPARE_TONE_AND_SILENCE)
168 #error You cant use BUSYDETECT_TONEONLY together with BUSYDETECT_COMPARE_TONE_AND_SILENCE
169 #endif
170
171 /* The CNG signal consists of the transmission of 1100 Hz for 1/2 second,
172  * followed by a 3 second silent (2100 Hz OFF) period.
173  */
174 #define FAX_TONE_CNG_FREQ       1100
175 #define FAX_TONE_CNG_DURATION   500
176 #define FAX_TONE_CNG_DB         16
177
178 /* This signal may be sent by the Terminating FAX machine anywhere between
179  * 1.8 to 2.5 seconds AFTER answering the call.  The CED signal consists
180  * of a 2100 Hz tone that is from 2.6 to 4 seconds in duration.
181 */
182 #define FAX_TONE_CED_FREQ       2100
183 #define FAX_TONE_CED_DURATION   2600
184 #define FAX_TONE_CED_DB         16
185
186 #define SAMPLE_RATE             8000
187
188 /* How many samples a frame has.  This constant is used when calculating
189  * Goertzel block size for tone_detect.  It is only important if we want to
190  * remove (squelch) the tone. In this case it is important to have block
191  * size not to exceed size of voice frame.  Otherwise by the moment the tone
192  * is detected it is too late to squelch it from previous frames.
193  */
194 #define SAMPLES_IN_FRAME        160
195
196 /* MF goertzel size */
197 #define MF_GSIZE                120
198
199 /* DTMF goertzel size */
200 #define DTMF_GSIZE              102
201
202 /* How many successive hits needed to consider begin of a digit */
203 #define DTMF_HITS_TO_BEGIN      2
204 /* How many successive misses needed to consider end of a digit */
205 #define DTMF_MISSES_TO_END      3
206
207 #define CONFIG_FILE_NAME "dsp.conf"
208
209 typedef struct {
210         int v2;
211         int v3;
212         int chunky;
213         int fac;
214         int samples;
215 } goertzel_state_t;
216
217 typedef struct {
218         int value;
219         int power;
220 } goertzel_result_t;
221
222 typedef struct
223 {
224         int freq;
225         int block_size;
226         int squelch;            /* Remove (squelch) tone */
227         goertzel_state_t tone;
228         float energy;           /* Accumulated energy of the current block */
229         int samples_pending;    /* Samples remain to complete the current block */
230         int mute_samples;       /* How many additional samples needs to be muted to suppress already detected tone */
231
232         int hits_required;      /* How many successive blocks with tone we are looking for */
233         float threshold;        /* Energy of the tone relative to energy from all other signals to consider a hit */
234
235         int hit_count;          /* How many successive blocks we consider tone present */
236         int last_hit;           /* Indicates if the last processed block was a hit */
237
238 } tone_detect_state_t;
239
240 typedef struct
241 {
242         goertzel_state_t row_out[4];
243         goertzel_state_t col_out[4];
244         int hits_to_begin;              /* How many successive hits needed to consider begin of a digit */
245         int misses_to_end;              /* How many successive misses needed to consider end of a digit */
246         int hits;                       /* How many successive hits we have seen already */
247         int misses;                     /* How many successive misses we have seen already */
248         int lasthit;
249         int current_hit;
250         float energy;
251         int current_sample;
252         int mute_samples;
253 } dtmf_detect_state_t;
254
255 typedef struct
256 {
257         goertzel_state_t tone_out[6];
258         int current_hit;
259         int hits[5];
260         int current_sample;
261         int mute_samples;
262 } mf_detect_state_t;
263
264 typedef struct
265 {
266         char digits[MAX_DTMF_DIGITS + 1];
267         int current_digits;
268         int detected_digits;
269         int lost_digits;
270
271         union {
272                 dtmf_detect_state_t dtmf;
273                 mf_detect_state_t mf;
274         } td;
275 } digit_detect_state_t;
276
277 static float dtmf_row[] =
278 {
279         697.0,  770.0,  852.0,  941.0
280 };
281 static float dtmf_col[] =
282 {
283         1209.0, 1336.0, 1477.0, 1633.0
284 };
285
286 static float mf_tones[] =
287 {
288         700.0, 900.0, 1100.0, 1300.0, 1500.0, 1700.0
289 };
290
291 static char dtmf_positions[] = "123A" "456B" "789C" "*0#D";
292
293 static char bell_mf_positions[] = "1247C-358A--69*---0B----#";
294
295 static int thresholds[THRESHOLD_MAX];
296
297 static inline void goertzel_sample(goertzel_state_t *s, short sample)
298 {
299         int v1;
300         
301         v1 = s->v2;
302         s->v2 = s->v3;
303         
304         s->v3 = (s->fac * s->v2) >> 15;
305         s->v3 = s->v3 - v1 + (sample >> s->chunky);
306         if (abs(s->v3) > 32768) {
307                 s->chunky++;
308                 s->v3 = s->v3 >> 1;
309                 s->v2 = s->v2 >> 1;
310                 v1 = v1 >> 1;
311         }
312 }
313
314 static inline void goertzel_update(goertzel_state_t *s, short *samps, int count)
315 {
316         int i;
317         
318         for (i=0;i<count;i++) 
319                 goertzel_sample(s, samps[i]);
320 }
321
322
323 static inline float goertzel_result(goertzel_state_t *s)
324 {
325         goertzel_result_t r;
326         r.value = (s->v3 * s->v3) + (s->v2 * s->v2);
327         r.value -= ((s->v2 * s->v3) >> 15) * s->fac;
328         r.power = s->chunky * 2;
329         return (float)r.value * (float)(1 << r.power);
330 }
331
332 static inline void goertzel_init(goertzel_state_t *s, float freq, int samples)
333 {
334         s->v2 = s->v3 = s->chunky = 0.0;
335         s->fac = (int)(32768.0 * 2.0 * cos(2.0 * M_PI * freq / SAMPLE_RATE));
336         s->samples = samples;
337 }
338
339 static inline void goertzel_reset(goertzel_state_t *s)
340 {
341         s->v2 = s->v3 = s->chunky = 0.0;
342 }
343
344 typedef struct {
345         int start;
346         int end;
347 } fragment_t;
348
349 /* Note on tone suppression (squelching). Individual detectors (DTMF/MF/generic tone)
350  * report fragmens of the frame in which detected tone resides and which needs
351  * to be "muted" in order to suppress the tone. To mark fragment for muting,
352  * detectors call mute_fragment passing fragment_t there. Multiple fragments
353  * can be marked and ast_dsp_process later will mute all of them.
354  *
355  * Note: When tone starts in the middle of a Goertzel block, it won't be properly
356  * detected in that block, only in the next. If we only mute the next block
357  * where tone is actually detected, the user will still hear beginning
358  * of the tone in preceeding block. This is why we usually want to mute some amount
359  * of samples preceeding and following the block where tone was detected.
360 */
361
362 struct ast_dsp {
363         struct ast_frame f;
364         int threshold;
365         int totalsilence;
366         int totalnoise;
367         int features;
368         int ringtimeout;
369         int busymaybe;
370         int busycount;
371         int busy_tonelength;
372         int busy_quietlength;
373         int historicnoise[DSP_HISTORY];
374         int historicsilence[DSP_HISTORY];
375         goertzel_state_t freqs[7];
376         int freqcount;
377         int gsamps;
378         enum gsamp_size gsamp_size;
379         enum prog_mode progmode;
380         int tstate;
381         int tcount;
382         int digitmode;
383         int faxmode;
384         int dtmf_began;
385         float genergy;
386         int mute_fragments;
387         fragment_t mute_data[5];
388         digit_detect_state_t digit_state;
389         tone_detect_state_t cng_tone_state;
390         tone_detect_state_t ced_tone_state;
391 };
392
393 static void mute_fragment(struct ast_dsp *dsp, fragment_t *fragment)
394 {
395         if (dsp->mute_fragments >= sizeof(dsp->mute_data) / sizeof(dsp->mute_data[0])) {
396                 ast_log(LOG_ERROR, "Too many fragments to mute. Ignoring\n");
397                 return;
398         }
399
400         dsp->mute_data[dsp->mute_fragments++] = *fragment;
401 }
402
403 static void ast_tone_detect_init(tone_detect_state_t *s, int freq, int duration, int amp)
404 {
405         int duration_samples;
406         float x;
407         int periods_in_block;
408
409         s->freq = freq;
410
411         /* Desired tone duration in samples */
412         duration_samples = duration * SAMPLE_RATE / 1000;
413         /* We want to allow 10% deviation of tone duration */
414         duration_samples = duration_samples * 9 / 10;
415
416         /* If we want to remove tone, it is important to have block size not
417            to exceed frame size. Otherwise by the moment tone is detected it is too late
418            to squelch it from previous frames */
419         s->block_size = SAMPLES_IN_FRAME;
420
421         periods_in_block = s->block_size * freq / SAMPLE_RATE;
422
423         /* Make sure we will have at least 5 periods at target frequency for analisys.
424            This may make block larger than expected packet and will make squelching impossible
425            but at least we will be detecting the tone */
426         if (periods_in_block < 5)
427                 periods_in_block = 5;
428
429         /* Now calculate final block size. It will contain integer number of periods */
430         s->block_size = periods_in_block * SAMPLE_RATE / freq;
431
432         /* tone_detect is currently only used to detect fax tones and we
433            do not need suqlching the fax tones */
434         s->squelch = 0;
435
436         /* Account for the first and the last block to be incomplete
437            and thus no tone will be detected in them */
438         s->hits_required = (duration_samples - (s->block_size - 1)) / s->block_size;
439
440         goertzel_init(&s->tone, freq, s->block_size);
441
442         s->samples_pending = s->block_size;
443         s->hit_count = 0;
444         s->last_hit = 0;
445         s->energy = 0.0;
446
447         /* We want tone energy to be amp decibels above the rest of the signal (the noise).
448            According to Parseval's theorem the energy computed in time domain equals to energy
449            computed in frequency domain. So subtracting energy in the frequency domain (Goertzel result)
450            from the energy in the time domain we will get energy of the remaining signal (without the tone
451            we are detecting). We will be checking that
452                 10*log(Ew / (Et - Ew)) > amp
453            Calculate threshold so that we will be actually checking
454                 Ew > Et * threshold
455         */
456
457         x = pow(10.0, amp / 10.0);
458         s->threshold = x / (x + 1);
459
460         ast_debug(1, "Setup tone %d Hz, %d ms, block_size=%d, hits_required=%d\n", freq, duration, s->block_size, s->hits_required);
461 }
462
463 static void ast_fax_detect_init(struct ast_dsp *s)
464 {
465         ast_tone_detect_init(&s->cng_tone_state, FAX_TONE_CNG_FREQ, FAX_TONE_CNG_DURATION, FAX_TONE_CNG_DB);
466         ast_tone_detect_init(&s->ced_tone_state, FAX_TONE_CED_FREQ, FAX_TONE_CED_DURATION, FAX_TONE_CED_DB);
467 }
468
469 static void ast_dtmf_detect_init (dtmf_detect_state_t *s)
470 {
471         int i;
472
473         s->lasthit = 0;
474         s->current_hit = 0;
475         for (i = 0;  i < 4;  i++) {
476                 goertzel_init (&s->row_out[i], dtmf_row[i], DTMF_GSIZE);
477                 goertzel_init (&s->col_out[i], dtmf_col[i], DTMF_GSIZE);
478                 s->energy = 0.0;
479         }
480         s->current_sample = 0;
481         s->hits = 0;
482         s->misses = 0;
483
484         s->hits_to_begin = DTMF_HITS_TO_BEGIN;
485         s->misses_to_end = DTMF_MISSES_TO_END;
486 }
487
488 static void ast_mf_detect_init (mf_detect_state_t *s)
489 {
490         int i;
491         s->hits[0] = s->hits[1] = s->hits[2] = s->hits[3] = s->hits[4] = 0;
492         for (i = 0;  i < 6;  i++) {
493                 goertzel_init (&s->tone_out[i], mf_tones[i], 160);
494         }
495         s->current_sample = 0;
496         s->current_hit = 0;
497 }
498
499 static void ast_digit_detect_init(digit_detect_state_t *s, int mf)
500 {
501         s->current_digits = 0;
502         s->detected_digits = 0;
503         s->lost_digits = 0;
504         s->digits[0] = '\0';
505
506         if (mf)
507                 ast_mf_detect_init(&s->td.mf);
508         else
509                 ast_dtmf_detect_init(&s->td.dtmf);
510 }
511
512 static int tone_detect(struct ast_dsp *dsp, tone_detect_state_t *s, int16_t *amp, int samples)
513 {
514         float tone_energy;
515         int i;
516         int hit = 0;
517         int limit;
518         int res = 0;
519         int16_t *ptr;
520         int start, end;
521         fragment_t mute = {0, 0};
522
523         if (s->squelch && s->mute_samples > 0) {
524                 mute.end = (s->mute_samples < samples) ? s->mute_samples : samples;
525                 s->mute_samples -= mute.end;
526         }
527
528         for (start = 0;  start < samples;  start = end) {
529                 /* Process in blocks. */
530                 limit = samples - start;
531                 if (limit > s->samples_pending)
532                         limit = s->samples_pending;
533                 end = start + limit;
534
535                 for (i = limit, ptr = amp ; i > 0; i--, ptr++) {
536                         /* signed 32 bit int should be enough to suqare any possible signed 16 bit value */
537                         s->energy += (int32_t) *ptr * (int32_t) *ptr;
538
539                         goertzel_sample(&s->tone, *ptr);
540                 }
541
542                 s->samples_pending -= limit;
543
544                 if (s->samples_pending) {
545                         /* Finished incomplete (last) block */
546                         break;
547                 }
548
549                 tone_energy = goertzel_result(&s->tone);
550
551                 /* Scale to make comparable */
552                 tone_energy *= 2.0;
553                 s->energy *= s->block_size;
554
555                 ast_debug(10, "tone %d, Ew=%.2E, Et=%.2E, s/n=%10.2f\n", s->freq, tone_energy, s->energy, tone_energy / (s->energy - tone_energy));
556                 hit = 0;
557                 if (tone_energy > s->energy * s->threshold) {
558
559                         ast_debug(10, "Hit! count=%d\n", s->hit_count);
560                         hit = 1;
561                 }
562
563                 if (s->hit_count)
564                         s->hit_count++;
565
566                 if (hit == s->last_hit) {
567                         if (!hit) {
568                                 /* Two successive misses. Tone ended */
569                                 s->hit_count = 0;
570                         } else if (!s->hit_count) {
571                                 s->hit_count++;
572                         }
573
574                 }
575
576                 if (s->hit_count == s->hits_required) {
577                         ast_debug(1, "%d Hz done detected\n", s->freq);
578                         res = 1;
579                 }
580
581                 s->last_hit = hit;
582
583                 /* If we had a hit in this block, include it into mute fragment */
584                 if (s->squelch && hit) {
585                         if (mute.end < start - s->block_size) {
586                                 /* There is a gap between fragments */
587                                 mute_fragment(dsp, &mute);
588                                 mute.start = (start > s->block_size) ? (start - s->block_size) : 0;
589                         }
590                         mute.end = end + s->block_size;
591                 }
592
593                 /* Reinitialise the detector for the next block */
594                 /* Reset for the next block */
595                 goertzel_reset(&s->tone);
596
597                 /* Advance to the next block */
598                 s->energy = 0.0;
599                 s->samples_pending = s->block_size;
600
601                 amp += limit;
602         }
603
604         if (s->squelch && mute.end) {
605                 if (mute.end > samples) {
606                         s->mute_samples = mute.end - samples;
607                         mute.end = samples;
608                 }
609                 mute_fragment(dsp, &mute);
610         }
611
612         return res;
613 }
614
615 static void store_digit(digit_detect_state_t *s, char digit)
616 {
617         s->detected_digits++;
618         if (s->current_digits < MAX_DTMF_DIGITS) {
619                 s->digits[s->current_digits++] = digit;
620                 s->digits[s->current_digits] = '\0';
621         } else {
622                 ast_log(LOG_WARNING, "Digit lost due to full buffer\n");
623                 s->lost_digits++;
624         }
625 }
626
627 static int dtmf_detect(struct ast_dsp *dsp, digit_detect_state_t *s, int16_t amp[], int samples, int squelch, int relax)
628 {
629         float row_energy[4];
630         float col_energy[4];
631         float famp;
632         int i;
633         int j;
634         int sample;
635         int best_row;
636         int best_col;
637         int hit;
638         int limit;
639         fragment_t mute = {0, 0};
640
641         if (squelch && s->td.dtmf.mute_samples > 0) {
642                 mute.end = (s->td.dtmf.mute_samples < samples) ? s->td.dtmf.mute_samples : samples;
643                 s->td.dtmf.mute_samples -= mute.end;
644         }
645
646         hit = 0;
647         for (sample = 0;  sample < samples;  sample = limit) {
648                 /* DTMF_GSIZE is optimised to meet the DTMF specs. */
649                 if ((samples - sample) >= (DTMF_GSIZE - s->td.dtmf.current_sample))
650                         limit = sample + (DTMF_GSIZE - s->td.dtmf.current_sample);
651                 else
652                         limit = samples;
653                 /* The following unrolled loop takes only 35% (rough estimate) of the 
654                    time of a rolled loop on the machine on which it was developed */
655                 for (j = sample; j < limit; j++) {
656                         famp = amp[j];
657                         s->td.dtmf.energy += famp*famp;
658                         /* With GCC 2.95, the following unrolled code seems to take about 35%
659                            (rough estimate) as long as a neat little 0-3 loop */
660                         goertzel_sample(s->td.dtmf.row_out, amp[j]);
661                         goertzel_sample(s->td.dtmf.col_out, amp[j]);
662                         goertzel_sample(s->td.dtmf.row_out + 1, amp[j]);
663                         goertzel_sample(s->td.dtmf.col_out + 1, amp[j]);
664                         goertzel_sample(s->td.dtmf.row_out + 2, amp[j]);
665                         goertzel_sample(s->td.dtmf.col_out + 2, amp[j]);
666                         goertzel_sample(s->td.dtmf.row_out + 3, amp[j]);
667                         goertzel_sample(s->td.dtmf.col_out + 3, amp[j]);
668                 }
669                 s->td.dtmf.current_sample += (limit - sample);
670                 if (s->td.dtmf.current_sample < DTMF_GSIZE) {
671                         continue;
672                 }
673                 /* We are at the end of a DTMF detection block */
674                 /* Find the peak row and the peak column */
675                 row_energy[0] = goertzel_result (&s->td.dtmf.row_out[0]);
676                 col_energy[0] = goertzel_result (&s->td.dtmf.col_out[0]);
677
678                 for (best_row = best_col = 0, i = 1;  i < 4;  i++) {
679                         row_energy[i] = goertzel_result (&s->td.dtmf.row_out[i]);
680                         if (row_energy[i] > row_energy[best_row])
681                                 best_row = i;
682                         col_energy[i] = goertzel_result (&s->td.dtmf.col_out[i]);
683                         if (col_energy[i] > col_energy[best_col])
684                                 best_col = i;
685                 }
686                 hit = 0;
687                 /* Basic signal level test and the twist test */
688                 if (row_energy[best_row] >= DTMF_THRESHOLD && 
689                     col_energy[best_col] >= DTMF_THRESHOLD &&
690                     col_energy[best_col] < row_energy[best_row]*DTMF_REVERSE_TWIST &&
691                     col_energy[best_col]*DTMF_NORMAL_TWIST > row_energy[best_row]) {
692                         /* Relative peak test */
693                         for (i = 0;  i < 4;  i++) {
694                                 if ((i != best_col &&
695                                     col_energy[i]*DTMF_RELATIVE_PEAK_COL > col_energy[best_col]) ||
696                                     (i != best_row 
697                                      && row_energy[i]*DTMF_RELATIVE_PEAK_ROW > row_energy[best_row])) {
698                                         break;
699                                 }
700                         }
701                         /* ... and fraction of total energy test */
702                         if (i >= 4 &&
703                             (row_energy[best_row] + col_energy[best_col]) > DTMF_TO_TOTAL_ENERGY*s->td.dtmf.energy) {
704                                 /* Got a hit */
705                                 hit = dtmf_positions[(best_row << 2) + best_col];
706                         }
707                 } 
708
709                 if (s->td.dtmf.current_hit) {
710                         /* We are in the middle of a digit already */
711                         if (hit != s->td.dtmf.current_hit) {
712                                 s->td.dtmf.misses++;
713                                 if (s->td.dtmf.misses == s->td.dtmf.misses_to_end) {
714                                         /* There were enough misses to consider digit ended */
715                                         s->td.dtmf.current_hit = 0;
716                                 }
717                         } else {
718                                 s->td.dtmf.misses = 0;
719                         }
720                 }
721
722                 /* Look for a start of a new digit no matter if we are already in the middle of some
723                    digit or not. This is because hits_to_begin may be smaller than misses_to_end
724                    and we may find begin of new digit before we consider last one ended. */
725                 if (hit) {
726                         if (hit == s->td.dtmf.lasthit) {
727                                 s->td.dtmf.hits++;
728                         } else {
729                                 s->td.dtmf.hits = 1;
730                         }
731
732                         if (s->td.dtmf.hits == s->td.dtmf.hits_to_begin && hit != s->td.dtmf.current_hit) {
733                                 store_digit(s, hit);
734                                 s->td.dtmf.current_hit = hit;
735                                 s->td.dtmf.misses = 0;
736                         }
737                 } else {
738                         s->td.dtmf.hits = 0;
739                 }
740
741                 s->td.dtmf.lasthit = hit;
742
743                 /* If we had a hit in this block, include it into mute fragment */
744                 if (squelch && hit) {
745                         if (mute.end < sample - DTMF_GSIZE) {
746                                 /* There is a gap between fragments */
747                                 mute_fragment(dsp, &mute);
748                                 mute.start = (sample > DTMF_GSIZE) ? (sample - DTMF_GSIZE) : 0;
749                         }
750                         mute.end = limit + DTMF_GSIZE;
751                 }
752
753                 /* Reinitialise the detector for the next block */
754                 for (i = 0;  i < 4;  i++) {
755                         goertzel_reset(&s->td.dtmf.row_out[i]);
756                         goertzel_reset(&s->td.dtmf.col_out[i]);
757                 }
758                 s->td.dtmf.energy = 0.0;
759                 s->td.dtmf.current_sample = 0;
760         }
761
762         if (squelch && mute.end) {
763                 if (mute.end > samples) {
764                         s->td.dtmf.mute_samples = mute.end - samples;
765                         mute.end = samples;
766                 }
767                 mute_fragment(dsp, &mute);
768         }
769
770         return (s->td.dtmf.current_hit);        /* return the debounced hit */
771 }
772
773 static int mf_detect(struct ast_dsp *dsp, digit_detect_state_t *s, int16_t amp[],
774                  int samples, int squelch, int relax)
775 {
776         float energy[6];
777         int best;
778         int second_best;
779         float famp;
780         int i;
781         int j;
782         int sample;
783         int hit;
784         int limit;
785         fragment_t mute = {0, 0};
786
787         if (squelch && s->td.mf.mute_samples > 0) {
788                 mute.end = (s->td.mf.mute_samples < samples) ? s->td.mf.mute_samples : samples;
789                 s->td.mf.mute_samples -= mute.end;
790         }
791
792         hit = 0;
793         for (sample = 0;  sample < samples;  sample = limit) {
794                 /* 80 is optimised to meet the MF specs. */
795                 /* XXX So then why is MF_GSIZE defined as 120? */
796                 if ((samples - sample) >= (MF_GSIZE - s->td.mf.current_sample))
797                         limit = sample + (MF_GSIZE - s->td.mf.current_sample);
798                 else
799                         limit = samples;
800                 /* The following unrolled loop takes only 35% (rough estimate) of the 
801                    time of a rolled loop on the machine on which it was developed */
802                 for (j = sample;  j < limit;  j++) {
803                         famp = amp[j];
804                         /* With GCC 2.95, the following unrolled code seems to take about 35%
805                            (rough estimate) as long as a neat little 0-3 loop */
806                         goertzel_sample(s->td.mf.tone_out, amp[j]);
807                         goertzel_sample(s->td.mf.tone_out + 1, amp[j]);
808                         goertzel_sample(s->td.mf.tone_out + 2, amp[j]);
809                         goertzel_sample(s->td.mf.tone_out + 3, amp[j]);
810                         goertzel_sample(s->td.mf.tone_out + 4, amp[j]);
811                         goertzel_sample(s->td.mf.tone_out + 5, amp[j]);
812                 }
813                 s->td.mf.current_sample += (limit - sample);
814                 if (s->td.mf.current_sample < MF_GSIZE) {
815                         continue;
816                 }
817                 /* We're at the end of an MF detection block.  */
818                 /* Find the two highest energies. The spec says to look for
819                    two tones and two tones only. Taking this literally -ie
820                    only two tones pass the minimum threshold - doesn't work
821                    well. The sinc function mess, due to rectangular windowing
822                    ensure that! Find the two highest energies and ensure they
823                    are considerably stronger than any of the others. */
824                 energy[0] = goertzel_result(&s->td.mf.tone_out[0]);
825                 energy[1] = goertzel_result(&s->td.mf.tone_out[1]);
826                 if (energy[0] > energy[1]) {
827                         best = 0;
828                         second_best = 1;
829                 } else {
830                         best = 1;
831                         second_best = 0;
832                 }
833                 /*endif*/
834                 for (i=2;i<6;i++) {
835                         energy[i] = goertzel_result(&s->td.mf.tone_out[i]);
836                         if (energy[i] >= energy[best]) {
837                                 second_best = best;
838                                 best = i;
839                         } else if (energy[i] >= energy[second_best]) {
840                                 second_best = i;
841                         }
842                 }
843                 /* Basic signal level and twist tests */
844                 hit = 0;
845                 if (energy[best] >= BELL_MF_THRESHOLD && energy[second_best] >= BELL_MF_THRESHOLD
846                     && energy[best] < energy[second_best]*BELL_MF_TWIST
847                     && energy[best]*BELL_MF_TWIST > energy[second_best]) {
848                         /* Relative peak test */
849                         hit = -1;
850                         for (i=0;i<6;i++) {
851                                 if (i != best && i != second_best) {
852                                         if (energy[i]*BELL_MF_RELATIVE_PEAK >= energy[second_best]) {
853                                                 /* The best two are not clearly the best */
854                                                 hit = 0;
855                                                 break;
856                                         }
857                                 }
858                         }
859                 }
860                 if (hit) {
861                         /* Get the values into ascending order */
862                         if (second_best < best) {
863                                 i = best;
864                                 best = second_best;
865                                 second_best = i;
866                         }
867                         best = best*5 + second_best - 1;
868                         hit = bell_mf_positions[best];
869                         /* Look for two successive similar results */
870                         /* The logic in the next test is:
871                            For KP we need 4 successive identical clean detects, with
872                            two blocks of something different preceeding it. For anything
873                            else we need two successive identical clean detects, with
874                            two blocks of something different preceeding it. */
875                         if (hit == s->td.mf.hits[4] && hit == s->td.mf.hits[3] &&
876                            ((hit != '*' && hit != s->td.mf.hits[2] && hit != s->td.mf.hits[1])||
877                             (hit == '*' && hit == s->td.mf.hits[2] && hit != s->td.mf.hits[1] && 
878                             hit != s->td.mf.hits[0]))) {
879                                 store_digit(s, hit);
880                         }
881                 }
882
883
884                 if (hit != s->td.mf.hits[4] && hit != s->td.mf.hits[3]) {
885                         /* Two successive block without a hit terminate current digit */
886                         s->td.mf.current_hit = 0;
887                 }
888
889                 s->td.mf.hits[0] = s->td.mf.hits[1];
890                 s->td.mf.hits[1] = s->td.mf.hits[2];
891                 s->td.mf.hits[2] = s->td.mf.hits[3];
892                 s->td.mf.hits[3] = s->td.mf.hits[4];
893                 s->td.mf.hits[4] = hit;
894
895                 /* If we had a hit in this block, include it into mute fragment */
896                 if (squelch && hit) {
897                         if (mute.end < sample - MF_GSIZE) {
898                                 /* There is a gap between fragments */
899                                 mute_fragment(dsp, &mute);
900                                 mute.start = (sample > MF_GSIZE) ? (sample - MF_GSIZE) : 0;
901                         }
902                         mute.end = limit + DTMF_GSIZE;
903                 }
904
905                 /* Reinitialise the detector for the next block */
906                 for (i = 0;  i < 6;  i++)
907                         goertzel_reset(&s->td.mf.tone_out[i]);
908                 s->td.mf.current_sample = 0;
909         }
910
911         if (squelch && mute.end) {
912                 if (mute.end > samples) {
913                         s->td.mf.mute_samples = mute.end - samples;
914                         mute.end = samples;
915                 }
916                 mute_fragment(dsp, &mute);
917         }
918
919         return (s->td.mf.current_hit); /* return the debounced hit */
920 }
921
922 static inline int pair_there(float p1, float p2, float i1, float i2, float e)
923 {
924         /* See if p1 and p2 are there, relative to i1 and i2 and total energy */
925         /* Make sure absolute levels are high enough */
926         if ((p1 < TONE_MIN_THRESH) || (p2 < TONE_MIN_THRESH))
927                 return 0;
928         /* Amplify ignored stuff */
929         i2 *= TONE_THRESH;
930         i1 *= TONE_THRESH;
931         e *= TONE_THRESH;
932         /* Check first tone */
933         if ((p1 < i1) || (p1 < i2) || (p1 < e))
934                 return 0;
935         /* And second */
936         if ((p2 < i1) || (p2 < i2) || (p2 < e))
937                 return 0;
938         /* Guess it's there... */
939         return 1;
940 }
941
942 static int __ast_dsp_call_progress(struct ast_dsp *dsp, short *s, int len)
943 {
944         int x;
945         int y;
946         int pass;
947         int newstate = DSP_TONE_STATE_SILENCE;
948         int res = 0;
949         while (len) {
950                 /* Take the lesser of the number of samples we need and what we have */
951                 pass = len;
952                 if (pass > dsp->gsamp_size - dsp->gsamps) 
953                         pass = dsp->gsamp_size - dsp->gsamps;
954                 for (x=0;x<pass;x++) {
955                         for (y=0;y<dsp->freqcount;y++) 
956                                 goertzel_sample(&dsp->freqs[y], s[x]);
957                         dsp->genergy += s[x] * s[x];
958                 }
959                 s += pass;
960                 dsp->gsamps += pass;
961                 len -= pass;
962                 if (dsp->gsamps == dsp->gsamp_size) {
963                         float hz[7];
964                         for (y=0;y<7;y++)
965                                 hz[y] = goertzel_result(&dsp->freqs[y]);
966                         switch (dsp->progmode) {
967                         case PROG_MODE_NA:
968                                 if (pair_there(hz[HZ_480], hz[HZ_620], hz[HZ_350], hz[HZ_440], dsp->genergy)) {
969                                         newstate = DSP_TONE_STATE_BUSY;
970                                 } else if (pair_there(hz[HZ_440], hz[HZ_480], hz[HZ_350], hz[HZ_620], dsp->genergy)) {
971                                         newstate = DSP_TONE_STATE_RINGING;
972                                 } else if (pair_there(hz[HZ_350], hz[HZ_440], hz[HZ_480], hz[HZ_620], dsp->genergy)) {
973                                         newstate = DSP_TONE_STATE_DIALTONE;
974                                 } else if (hz[HZ_950] > TONE_MIN_THRESH * TONE_THRESH) {
975                                         newstate = DSP_TONE_STATE_SPECIAL1;
976                                 } else if (hz[HZ_1400] > TONE_MIN_THRESH * TONE_THRESH) {
977                                         if (dsp->tstate == DSP_TONE_STATE_SPECIAL1)
978                                                 newstate = DSP_TONE_STATE_SPECIAL2;
979                                 } else if (hz[HZ_1800] > TONE_MIN_THRESH * TONE_THRESH) {
980                                         if (dsp->tstate == DSP_TONE_STATE_SPECIAL2)
981                                                 newstate = DSP_TONE_STATE_SPECIAL3;
982                                 } else if (dsp->genergy > TONE_MIN_THRESH * TONE_THRESH) {
983                                         newstate = DSP_TONE_STATE_TALKING;
984                                 } else
985                                         newstate = DSP_TONE_STATE_SILENCE;
986                                 break;
987                         case PROG_MODE_CR:
988                                 if (hz[HZ_425] > TONE_MIN_THRESH * TONE_THRESH) {
989                                         newstate = DSP_TONE_STATE_RINGING;
990                                 } else if (dsp->genergy > TONE_MIN_THRESH * TONE_THRESH) {
991                                         newstate = DSP_TONE_STATE_TALKING;
992                                 } else
993                                         newstate = DSP_TONE_STATE_SILENCE;
994                                 break;
995                         case PROG_MODE_UK:
996                                 if (hz[HZ_400] > TONE_MIN_THRESH * TONE_THRESH) {
997                                         newstate = DSP_TONE_STATE_HUNGUP;
998                                 }
999                                 break;
1000                         default:
1001                                 ast_log(LOG_WARNING, "Can't process in unknown prog mode '%d'\n", dsp->progmode);
1002                         }
1003                         if (newstate == dsp->tstate) {
1004                                 dsp->tcount++;
1005                                 if (dsp->ringtimeout)
1006                                         dsp->ringtimeout++;
1007                                 switch (dsp->tstate) {
1008                                         case DSP_TONE_STATE_RINGING:
1009                                                 if ((dsp->features & DSP_PROGRESS_RINGING) &&
1010                                                     (dsp->tcount==THRESH_RING)) {
1011                                                         res = AST_CONTROL_RINGING;
1012                                                         dsp->ringtimeout= 1;
1013                                                 }
1014                                                 break;
1015                                         case DSP_TONE_STATE_BUSY:
1016                                                 if ((dsp->features & DSP_PROGRESS_BUSY) &&
1017                                                     (dsp->tcount==THRESH_BUSY)) {
1018                                                         res = AST_CONTROL_BUSY;
1019                                                         dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
1020                                                 }
1021                                                 break;
1022                                         case DSP_TONE_STATE_TALKING:
1023                                                 if ((dsp->features & DSP_PROGRESS_TALK) &&
1024                                                     (dsp->tcount==THRESH_TALK)) {
1025                                                         res = AST_CONTROL_ANSWER;
1026                                                         dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
1027                                                 }
1028                                                 break;
1029                                         case DSP_TONE_STATE_SPECIAL3:
1030                                                 if ((dsp->features & DSP_PROGRESS_CONGESTION) &&
1031                                                     (dsp->tcount==THRESH_CONGESTION)) {
1032                                                         res = AST_CONTROL_CONGESTION;
1033                                                         dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
1034                                                 }
1035                                                 break;
1036                                         case DSP_TONE_STATE_HUNGUP:
1037                                                 if ((dsp->features & DSP_FEATURE_CALL_PROGRESS) &&
1038                                                     (dsp->tcount==THRESH_HANGUP)) {
1039                                                         res = AST_CONTROL_HANGUP;
1040                                                         dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
1041                                                 }
1042                                                 break;
1043                                 }
1044                                 if (dsp->ringtimeout==THRESH_RING2ANSWER) {
1045                                         ast_debug(1, "Consider call as answered because of timeout after last ring\n");
1046                                         res = AST_CONTROL_ANSWER;
1047                                         dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
1048                                 }
1049                         } else {
1050                                 ast_debug(5, "Stop state %d with duration %d\n", dsp->tstate, dsp->tcount);
1051                                 ast_debug(5, "Start state %d\n", newstate);
1052                                 dsp->tstate = newstate;
1053                                 dsp->tcount = 1;
1054                         }
1055                         
1056                         /* Reset goertzel */                                            
1057                         for (x=0;x<7;x++)
1058                                 dsp->freqs[x].v2 = dsp->freqs[x].v3 = 0.0;
1059                         dsp->gsamps = 0;
1060                         dsp->genergy = 0.0;
1061                 }
1062         }
1063
1064         return res;
1065 }
1066
1067 int ast_dsp_call_progress(struct ast_dsp *dsp, struct ast_frame *inf)
1068 {
1069         if (inf->frametype != AST_FRAME_VOICE) {
1070                 ast_log(LOG_WARNING, "Can't check call progress of non-voice frames\n");
1071                 return 0;
1072         }
1073         if (inf->subclass != AST_FORMAT_SLINEAR) {
1074                 ast_log(LOG_WARNING, "Can only check call progress in signed-linear frames\n");
1075                 return 0;
1076         }
1077         return __ast_dsp_call_progress(dsp, inf->data, inf->datalen / 2);
1078 }
1079
1080 static int __ast_dsp_silence_noise(struct ast_dsp *dsp, short *s, int len, int *totalsilence, int *totalnoise)
1081 {
1082         int accum;
1083         int x;
1084         int res = 0;
1085
1086         if (!len)
1087                 return 0;
1088         accum = 0;
1089         for (x=0;x<len; x++) 
1090                 accum += abs(s[x]);
1091         accum /= len;
1092         if (accum < dsp->threshold) {
1093                 /* Silent */
1094                 dsp->totalsilence += len/8;
1095                 if (dsp->totalnoise) {
1096                         /* Move and save history */
1097                         memmove(dsp->historicnoise + DSP_HISTORY - dsp->busycount, dsp->historicnoise + DSP_HISTORY - dsp->busycount +1, dsp->busycount*sizeof(dsp->historicnoise[0]));
1098                         dsp->historicnoise[DSP_HISTORY - 1] = dsp->totalnoise;
1099 /* we don't want to check for busydetect that frequently */
1100 #if 0
1101                         dsp->busymaybe = 1;
1102 #endif
1103                 }
1104                 dsp->totalnoise = 0;
1105                 res = 1;
1106         } else {
1107                 /* Not silent */
1108                 dsp->totalnoise += len/8;
1109                 if (dsp->totalsilence) {
1110                         int silence1 = dsp->historicsilence[DSP_HISTORY - 1];
1111                         int silence2 = dsp->historicsilence[DSP_HISTORY - 2];
1112                         /* Move and save history */
1113                         memmove(dsp->historicsilence + DSP_HISTORY - dsp->busycount, dsp->historicsilence + DSP_HISTORY - dsp->busycount + 1, dsp->busycount*sizeof(dsp->historicsilence[0]));
1114                         dsp->historicsilence[DSP_HISTORY - 1] = dsp->totalsilence;
1115                         /* check if the previous sample differs only by BUSY_PERCENT from the one before it */
1116                         if (silence1 < silence2) {
1117                                 if (silence1 + silence1*BUSY_PERCENT/100 >= silence2)
1118                                         dsp->busymaybe = 1;
1119                                 else 
1120                                         dsp->busymaybe = 0;
1121                         } else {
1122                                 if (silence1 - silence1*BUSY_PERCENT/100 <= silence2)
1123                                         dsp->busymaybe = 1;
1124                                 else 
1125                                         dsp->busymaybe = 0;
1126                         }
1127                 }
1128                 dsp->totalsilence = 0;
1129         }
1130         if (totalsilence)
1131                 *totalsilence = dsp->totalsilence;
1132         if (totalnoise)
1133                 *totalnoise = dsp->totalnoise;
1134         return res;
1135 }
1136
1137 int ast_dsp_busydetect(struct ast_dsp *dsp)
1138 {
1139         int res = 0, x;
1140 #ifndef BUSYDETECT_TONEONLY
1141         int avgsilence = 0, hitsilence = 0;
1142 #endif
1143         int avgtone = 0, hittone = 0;
1144         if (!dsp->busymaybe)
1145                 return res;
1146         for (x=DSP_HISTORY - dsp->busycount;x<DSP_HISTORY;x++) {
1147 #ifndef BUSYDETECT_TONEONLY
1148                 avgsilence += dsp->historicsilence[x];
1149 #endif
1150                 avgtone += dsp->historicnoise[x];
1151         }
1152 #ifndef BUSYDETECT_TONEONLY
1153         avgsilence /= dsp->busycount;
1154 #endif
1155         avgtone /= dsp->busycount;
1156         for (x=DSP_HISTORY - dsp->busycount;x<DSP_HISTORY;x++) {
1157 #ifndef BUSYDETECT_TONEONLY
1158                 if (avgsilence > dsp->historicsilence[x]) {
1159                         if (avgsilence - (avgsilence*BUSY_PERCENT/100) <= dsp->historicsilence[x])
1160                                 hitsilence++;
1161                 } else {
1162                         if (avgsilence + (avgsilence*BUSY_PERCENT/100) >= dsp->historicsilence[x])
1163                                 hitsilence++;
1164                 }
1165 #endif
1166                 if (avgtone > dsp->historicnoise[x]) {
1167                         if (avgtone - (avgtone*BUSY_PERCENT/100) <= dsp->historicnoise[x])
1168                                 hittone++;
1169                 } else {
1170                         if (avgtone + (avgtone*BUSY_PERCENT/100) >= dsp->historicnoise[x])
1171                                 hittone++;
1172                 }
1173         }
1174 #ifndef BUSYDETECT_TONEONLY
1175         if ((hittone >= dsp->busycount - 1) && (hitsilence >= dsp->busycount - 1) && 
1176             (avgtone >= BUSY_MIN && avgtone <= BUSY_MAX) && 
1177             (avgsilence >= BUSY_MIN && avgsilence <= BUSY_MAX)) {
1178 #else
1179         if ((hittone >= dsp->busycount - 1) && (avgtone >= BUSY_MIN && avgtone <= BUSY_MAX)) {
1180 #endif
1181 #ifdef BUSYDETECT_COMPARE_TONE_AND_SILENCE
1182                 if (avgtone > avgsilence) {
1183                         if (avgtone - avgtone*BUSY_PERCENT/100 <= avgsilence)
1184                                 res = 1;
1185                 } else {
1186                         if (avgtone + avgtone*BUSY_PERCENT/100 >= avgsilence)
1187                                 res = 1;
1188                 }
1189 #else
1190                 res = 1;
1191 #endif
1192         }
1193         /* If we know the expected busy tone length, check we are in the range */
1194         if (res && (dsp->busy_tonelength > 0)) {
1195                 if (abs(avgtone - dsp->busy_tonelength) > (dsp->busy_tonelength*BUSY_PAT_PERCENT/100)) {
1196 #ifdef BUSYDETECT_DEBUG
1197                         ast_debug(5, "busy detector: avgtone of %d not close enough to desired %d\n",
1198                                 avgtone, dsp->busy_tonelength);
1199 #endif
1200                         res = 0;
1201                 }
1202         }
1203 #ifndef BUSYDETECT_TONEONLY
1204         /* If we know the expected busy tone silent-period length, check we are in the range */
1205         if (res && (dsp->busy_quietlength > 0)) {
1206                 if (abs(avgsilence - dsp->busy_quietlength) > (dsp->busy_quietlength*BUSY_PAT_PERCENT/100)) {
1207 #ifdef BUSYDETECT_DEBUG
1208                 ast_debug(5, "busy detector: avgsilence of %d not close enough to desired %d\n",
1209                         avgsilence, dsp->busy_quietlength);
1210 #endif
1211                         res = 0;
1212                 }
1213         }
1214 #endif
1215 #if !defined(BUSYDETECT_TONEONLY) && defined(BUSYDETECT_DEBUG)
1216         if (res) {
1217                 ast_debug(5, "ast_dsp_busydetect detected busy, avgtone: %d, avgsilence %d\n", avgtone, avgsilence);
1218         } else {
1219                 ast_debug(5, "busy detector: FAILED with avgtone: %d, avgsilence %d\n", avgtone, avgsilence);
1220         }
1221 #endif
1222         return res;
1223 }
1224
1225 int ast_dsp_silence(struct ast_dsp *dsp, struct ast_frame *f, int *totalsilence)
1226 {
1227         short *s;
1228         int len;
1229         
1230         if (f->frametype != AST_FRAME_VOICE) {
1231                 ast_log(LOG_WARNING, "Can't calculate silence on a non-voice frame\n");
1232                 return 0;
1233         }
1234         if (f->subclass != AST_FORMAT_SLINEAR) {
1235                 ast_log(LOG_WARNING, "Can only calculate silence on signed-linear frames :(\n");
1236                 return 0;
1237         }
1238         s = f->data;
1239         len = f->datalen/2;
1240         return __ast_dsp_silence_noise(dsp, s, len, totalsilence, NULL);
1241 }
1242
1243 int ast_dsp_noise(struct ast_dsp *dsp, struct ast_frame *f, int *totalnoise)
1244 {
1245        short *s;
1246        int len;
1247
1248        if (f->frametype != AST_FRAME_VOICE) {
1249                ast_log(LOG_WARNING, "Can't calculate noise on a non-voice frame\n");
1250                return 0;
1251        }
1252        if (f->subclass != AST_FORMAT_SLINEAR) {
1253                ast_log(LOG_WARNING, "Can only calculate noise on signed-linear frames :(\n");
1254                return 0;
1255        }
1256        s = f->data;
1257        len = f->datalen/2;
1258        return __ast_dsp_silence_noise(dsp, s, len, NULL, totalnoise);
1259 }
1260
1261
1262 struct ast_frame *ast_dsp_process(struct ast_channel *chan, struct ast_dsp *dsp, struct ast_frame *af)
1263 {
1264         int silence;
1265         int res;
1266         int digit = 0, fax_digit = 0;
1267         int x;
1268         short *shortdata;
1269         unsigned char *odata;
1270         int len;
1271         struct ast_frame *outf = NULL;
1272
1273         if (!af)
1274                 return NULL;
1275         if (af->frametype != AST_FRAME_VOICE)
1276                 return af;
1277
1278         odata = af->data;
1279         len = af->datalen;
1280         /* Make sure we have short data */
1281         switch (af->subclass) {
1282         case AST_FORMAT_SLINEAR:
1283                 shortdata = af->data;
1284                 len = af->datalen / 2;
1285                 break;
1286         case AST_FORMAT_ULAW:
1287                 shortdata = alloca(af->datalen * 2);
1288                 for (x = 0;x < len; x++) 
1289                         shortdata[x] = AST_MULAW(odata[x]);
1290                 break;
1291         case AST_FORMAT_ALAW:
1292                 shortdata = alloca(af->datalen * 2);
1293                 for (x = 0; x < len; x++) 
1294                         shortdata[x] = AST_ALAW(odata[x]);
1295                 break;
1296         default:
1297                 ast_log(LOG_WARNING, "Inband DTMF is not supported on codec %s. Use RFC2833\n", ast_getformatname(af->subclass));
1298                 return af;
1299         }
1300
1301         /* Initially we do not want to mute anything */
1302         dsp->mute_fragments = 0;
1303
1304         res = __ast_dsp_silence_noise(dsp, shortdata, len, &silence, NULL);
1305         if ((dsp->features & DSP_FEATURE_SILENCE_SUPPRESS) && silence) {
1306                 memset(&dsp->f, 0, sizeof(dsp->f));
1307                 dsp->f.frametype = AST_FRAME_NULL;
1308                 ast_frfree(af);
1309                 return &dsp->f;
1310         }
1311         if ((dsp->features & DSP_FEATURE_BUSY_DETECT) && ast_dsp_busydetect(dsp)) {
1312                 chan->_softhangup |= AST_SOFTHANGUP_DEV;
1313                 memset(&dsp->f, 0, sizeof(dsp->f));
1314                 dsp->f.frametype = AST_FRAME_CONTROL;
1315                 dsp->f.subclass = AST_CONTROL_BUSY;
1316                 ast_frfree(af);
1317                 ast_debug(1, "Requesting Hangup because the busy tone was detected on channel %s\n", chan->name);
1318                 return &dsp->f;
1319         }
1320
1321         if ((dsp->features & DSP_FEATURE_FAX_DETECT)) {
1322                 if ((dsp->faxmode & DSP_FAXMODE_DETECT_CNG) && tone_detect(dsp, &dsp->cng_tone_state, shortdata, len)) {
1323                         fax_digit = 'f';
1324                 }
1325
1326                 if ((dsp->faxmode & DSP_FAXMODE_DETECT_CED) && tone_detect(dsp, &dsp->ced_tone_state, shortdata, len)) {
1327                         fax_digit = 'e';
1328                 }
1329         }
1330
1331         if ((dsp->features & DSP_FEATURE_DIGIT_DETECT)) {
1332                 if ((dsp->digitmode & DSP_DIGITMODE_MF))
1333                         digit = mf_detect(dsp, &dsp->digit_state, shortdata, len, (dsp->digitmode & DSP_DIGITMODE_NOQUELCH) == 0, (dsp->digitmode & DSP_DIGITMODE_RELAXDTMF));
1334                 else
1335                         digit = dtmf_detect(dsp, &dsp->digit_state, shortdata, len, (dsp->digitmode & DSP_DIGITMODE_NOQUELCH) == 0, (dsp->digitmode & DSP_DIGITMODE_RELAXDTMF));
1336
1337                 if (dsp->digit_state.current_digits) {
1338                         int event = 0;
1339                         char event_digit = 0;
1340
1341                         if (!dsp->dtmf_began) {
1342                                 /* We have not reported DTMF_BEGIN for anything yet */
1343
1344                                 event = AST_FRAME_DTMF_BEGIN;
1345                                 event_digit = dsp->digit_state.digits[0];
1346                                 dsp->dtmf_began = 1;
1347
1348                         } else if (dsp->digit_state.current_digits > 1 || digit != dsp->digit_state.digits[0]) {
1349                                 /* Digit changed. This means digit we have reported with DTMF_BEGIN ended */
1350         
1351                                 event = AST_FRAME_DTMF_END;
1352                                 event_digit = dsp->digit_state.digits[0];
1353                                 memmove(dsp->digit_state.digits, dsp->digit_state.digits + 1, dsp->digit_state.current_digits);
1354                                 dsp->digit_state.current_digits--;
1355                                 dsp->dtmf_began = 0;
1356                         }
1357
1358                         if (event) {
1359                                 memset(&dsp->f, 0, sizeof(dsp->f));
1360                                 dsp->f.frametype = event;
1361                                 dsp->f.subclass = event_digit;
1362                                 outf = &dsp->f;
1363                                 goto done;
1364                         }
1365                 }
1366         }
1367
1368         if (fax_digit) {
1369                 /* Fax was detected - digit is either 'f' or 'e' */
1370
1371                 memset(&dsp->f, 0, sizeof(dsp->f));
1372                 dsp->f.frametype = AST_FRAME_DTMF;
1373                 dsp->f.subclass = fax_digit;
1374                 outf = &dsp->f;
1375                 goto done;
1376         }
1377
1378         if ((dsp->features & DSP_FEATURE_CALL_PROGRESS)) {
1379                 res = __ast_dsp_call_progress(dsp, shortdata, len);
1380                 if (res) {
1381                         switch (res) {
1382                         case AST_CONTROL_ANSWER:
1383                         case AST_CONTROL_BUSY:
1384                         case AST_CONTROL_RINGING:
1385                         case AST_CONTROL_CONGESTION:
1386                         case AST_CONTROL_HANGUP:
1387                                 memset(&dsp->f, 0, sizeof(dsp->f));
1388                                 dsp->f.frametype = AST_FRAME_CONTROL;
1389                                 dsp->f.subclass = res;
1390                                 dsp->f.src = "dsp_progress";
1391                                 if (chan) 
1392                                         ast_queue_frame(chan, &dsp->f);
1393                                 break;
1394                         default:
1395                                 ast_log(LOG_WARNING, "Don't know how to represent call progress message %d\n", res);
1396                         }
1397                 }
1398         }
1399
1400 done:
1401         /* Mute fragment of the frame */
1402         for (x = 0; x < dsp->mute_fragments; x++) {
1403                 memset(shortdata + dsp->mute_data[x].start, 0, sizeof(int16_t) * (dsp->mute_data[x].end - dsp->mute_data[x].start));
1404         }
1405
1406         switch (af->subclass) {
1407         case AST_FORMAT_SLINEAR:
1408                 break;
1409         case AST_FORMAT_ULAW:
1410                 for (x = 0; x < len; x++)
1411                         odata[x] = AST_LIN2MU((unsigned short) shortdata[x]);
1412                 break;
1413         case AST_FORMAT_ALAW:
1414                 for (x = 0; x < len; x++)
1415                         odata[x] = AST_LIN2A((unsigned short) shortdata[x]);
1416                 break;
1417         }
1418
1419         if (outf) {
1420                 if (chan) 
1421                         ast_queue_frame(chan, af);
1422                 ast_frfree(af);
1423                 return outf;
1424         } else {
1425                 return af;
1426         }
1427 }
1428
1429 static void ast_dsp_prog_reset(struct ast_dsp *dsp)
1430 {
1431         int max = 0;
1432         int x;
1433         
1434         dsp->gsamp_size = modes[dsp->progmode].size;
1435         dsp->gsamps = 0;
1436         for (x = 0; x < sizeof(modes[dsp->progmode].freqs) / sizeof(modes[dsp->progmode].freqs[0]); x++) {
1437                 if (modes[dsp->progmode].freqs[x]) {
1438                         goertzel_init(&dsp->freqs[x], (float)modes[dsp->progmode].freqs[x], dsp->gsamp_size);
1439                         max = x + 1;
1440                 }
1441         }
1442         dsp->freqcount = max;
1443         dsp->ringtimeout= 0;
1444 }
1445
1446 struct ast_dsp *ast_dsp_new(void)
1447 {
1448         struct ast_dsp *dsp;
1449         
1450         if ((dsp = ast_calloc(1, sizeof(*dsp)))) {              
1451                 dsp->threshold = DEFAULT_THRESHOLD;
1452                 dsp->features = DSP_FEATURE_SILENCE_SUPPRESS;
1453                 dsp->busycount = DSP_HISTORY;
1454                 dsp->digitmode = DSP_DIGITMODE_DTMF;
1455                 dsp->faxmode = DSP_FAXMODE_DETECT_CNG;
1456                 /* Initialize digit detector */
1457                 ast_digit_detect_init(&dsp->digit_state, dsp->digitmode & DSP_DIGITMODE_MF);
1458                 /* Initialize initial DSP progress detect parameters */
1459                 ast_dsp_prog_reset(dsp);
1460                 /* Initialize fax detector */
1461                 ast_fax_detect_init(dsp);
1462         }
1463         return dsp;
1464 }
1465
1466 void ast_dsp_set_features(struct ast_dsp *dsp, int features)
1467 {
1468         dsp->features = features;
1469 }
1470
1471 void ast_dsp_free(struct ast_dsp *dsp)
1472 {
1473         ast_free(dsp);
1474 }
1475
1476 void ast_dsp_set_threshold(struct ast_dsp *dsp, int threshold)
1477 {
1478         dsp->threshold = threshold;
1479 }
1480
1481 void ast_dsp_set_busy_count(struct ast_dsp *dsp, int cadences)
1482 {
1483         if (cadences < 4)
1484                 cadences = 4;
1485         if (cadences > DSP_HISTORY)
1486                 cadences = DSP_HISTORY;
1487         dsp->busycount = cadences;
1488 }
1489
1490 void ast_dsp_set_busy_pattern(struct ast_dsp *dsp, int tonelength, int quietlength)
1491 {
1492         dsp->busy_tonelength = tonelength;
1493         dsp->busy_quietlength = quietlength;
1494         ast_debug(1, "dsp busy pattern set to %d,%d\n", tonelength, quietlength);
1495 }
1496
1497 void ast_dsp_digitreset(struct ast_dsp *dsp)
1498 {
1499         int i;
1500         
1501         dsp->dtmf_began = 0;
1502         if (dsp->digitmode & DSP_DIGITMODE_MF) {
1503                 mf_detect_state_t *s = &dsp->digit_state.td.mf;
1504                 /* Reinitialise the detector for the next block */
1505                 for (i = 0;  i < 6;  i++) {
1506                         goertzel_reset(&s->tone_out[i]);
1507                 }
1508                 s->hits[4] = s->hits[3] = s->hits[2] = s->hits[1] = s->hits[0] = s->current_hit = 0;
1509                 s->current_sample = 0;
1510         } else {
1511                 dtmf_detect_state_t *s = &dsp->digit_state.td.dtmf;
1512                 /* Reinitialise the detector for the next block */
1513                 for (i = 0;  i < 4;  i++) {
1514                         goertzel_reset(&s->row_out[i]);
1515                         goertzel_reset(&s->col_out[i]);
1516                 }
1517                 s->lasthit = s->current_hit = 0;
1518                 s->energy = 0.0;
1519                 s->current_sample = 0;
1520                 s->hits = 0;
1521                 s->misses = 0;
1522         }
1523
1524         dsp->digit_state.digits[0] = '\0';
1525         dsp->digit_state.current_digits = 0;
1526 }
1527
1528 void ast_dsp_reset(struct ast_dsp *dsp)
1529 {
1530         int x;
1531         
1532         dsp->totalsilence = 0;
1533         dsp->gsamps = 0;
1534         for (x=0;x<4;x++)
1535                 dsp->freqs[x].v2 = dsp->freqs[x].v3 = 0.0;
1536         memset(dsp->historicsilence, 0, sizeof(dsp->historicsilence));
1537         memset(dsp->historicnoise, 0, sizeof(dsp->historicnoise));      
1538         dsp->ringtimeout= 0;
1539 }
1540
1541 int ast_dsp_set_digitmode(struct ast_dsp *dsp, int digitmode)
1542 {
1543         int new;
1544         int old;
1545         
1546         old = dsp->digitmode & (DSP_DIGITMODE_DTMF | DSP_DIGITMODE_MF | DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX);
1547         new = digitmode & (DSP_DIGITMODE_DTMF | DSP_DIGITMODE_MF | DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX);
1548         if (old != new) {
1549                 /* Must initialize structures if switching from MF to DTMF or vice-versa */
1550                 ast_digit_detect_init(&dsp->digit_state, new & DSP_DIGITMODE_MF);
1551         }
1552         dsp->digitmode = digitmode;
1553         return 0;
1554 }
1555
1556 int ast_dsp_set_faxmode(struct ast_dsp *dsp, int faxmode)
1557 {
1558         if (dsp->faxmode != faxmode) {
1559                 ast_fax_detect_init(dsp);
1560         }
1561         dsp->faxmode = faxmode;
1562         return 0;
1563 }
1564
1565 int ast_dsp_set_call_progress_zone(struct ast_dsp *dsp, char *zone)
1566 {
1567         int x;
1568         
1569         for (x = 0; x < ARRAY_LEN(aliases); x++) {
1570                 if (!strcasecmp(aliases[x].name, zone)) {
1571                         dsp->progmode = aliases[x].mode;
1572                         ast_dsp_prog_reset(dsp);
1573                         return 0;
1574                 }
1575         }
1576         return -1;
1577 }
1578
1579 int ast_dsp_was_muted(struct ast_dsp *dsp)
1580 {
1581         return (dsp->mute_fragments > 0);
1582 }
1583
1584 int ast_dsp_get_tstate(struct ast_dsp *dsp) 
1585 {
1586         return dsp->tstate;
1587 }
1588
1589 int ast_dsp_get_tcount(struct ast_dsp *dsp) 
1590 {
1591         return dsp->tcount;
1592 }
1593
1594 static int _dsp_init(int reload)
1595 {
1596         struct ast_flags config_flags = { reload ? CONFIG_FLAG_FILEUNCHANGED : 0 };
1597         struct ast_config *cfg;
1598
1599         cfg = ast_config_load2(CONFIG_FILE_NAME, "dsp", config_flags);
1600
1601         if (cfg && cfg != CONFIG_STATUS_FILEUNCHANGED) {
1602                 const char *value;
1603
1604                 value = ast_variable_retrieve(cfg, "default", "silencethreshold");
1605                 if (value && sscanf(value, "%d", &thresholds[THRESHOLD_SILENCE]) != 1) {
1606                         ast_log(LOG_WARNING, "%s: '%s' is not a valid silencethreshold value\n", CONFIG_FILE_NAME, value);
1607                         thresholds[THRESHOLD_SILENCE] = 256;
1608                 } else if (!value)
1609                         thresholds[THRESHOLD_SILENCE] = 256;
1610
1611                 ast_config_destroy(cfg);
1612         }
1613         return 0;
1614 }
1615
1616 int ast_dsp_get_threshold_from_settings(enum threshold which)
1617 {
1618         return thresholds[which];
1619 }
1620
1621 int ast_dsp_init(void)
1622 {
1623         return _dsp_init(0);
1624 }
1625
1626 int ast_dsp_reload(void)
1627 {
1628         return _dsp_init(1);
1629 }
1630