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