--- main/dsp.c.orig 2008-01-31 09:32:09.000000000 +0300 +++ main/dsp.c 2008-01-31 23:59:32.000000000 +0300 @@ -149,9 +149,9 @@ #define FAX_2ND_HARMONIC 2.0 /* 4dB */ #define DTMF_NORMAL_TWIST 6.3 /* 8dB */ #ifdef RADIO_RELAX -#define DTMF_REVERSE_TWIST ((digitmode & DSP_DIGITMODE_RELAXDTMF) ? 6.5 : 2.5) /* 4dB normal */ +#define DTMF_REVERSE_TWIST (relax ? 6.5 : 2.5) /* 4dB normal */ #else -#define DTMF_REVERSE_TWIST ((digitmode & DSP_DIGITMODE_RELAXDTMF) ? 4.0 : 2.5) /* 4dB normal */ +#define DTMF_REVERSE_TWIST (relax ? 4.0 : 2.5) /* 4dB normal */ #endif #define DTMF_RELATIVE_PEAK_ROW 6.3 /* 8dB */ #define DTMF_RELATIVE_PEAK_COL 6.3 /* 8dB */ @@ -182,11 +182,13 @@ /* How many samples a frame has. This constant is used when calculating Goertzel block size for tone_detect. It is only important if we want to remove (squelch) the tone. In this case it is important to have block size not - to exceed size of voice fram. Otherwise by the moment tone is detected + to exceed size of voice frame. Otherwise by the moment tone is detected it is too late to squelch it from previous frames. */ #define SAMPLES_IN_FRAME 160 +/* DTMF goertzel size */ +#define DTMF_GSIZE 102 typedef struct { int v2; @@ -209,6 +211,7 @@ goertzel_state_t tone; float energy; /* Accumulated energy of the current block */ int samples_pending; /* Samples remain to complete the current block */ + int mute_samples; /* How many additional samples needs to be muted to suppress already detected tone */ int hits_required; /* How many successive blocks with tone we are looking for */ float threshold; /* Energy of the tone relative to energy from all other signals to consider a hit */ @@ -226,6 +229,7 @@ int current_hit; float energy; int current_sample; + int mute_samples; } dtmf_detect_state_t; typedef struct @@ -234,6 +238,7 @@ int current_hit; int hits[5]; int current_sample; + int mute_samples; } mf_detect_state_t; typedef struct @@ -314,6 +319,24 @@ s->v2 = s->v3 = s->chunky = 0.0; } +typedef struct { + int start; + int end; +} fragment_t; + +/* Note on tone suppression (squelching). Individual detectors (DTMF/MF/generic tone) + report fragmens of the frame in which detected tone resides and which needs + to be "muted" in order to suppress the tone. To mark fragment for muting, + detectors call mute_fragment passing fragment_t there. Multiple fragments + can be marked and ast_dsp_process later will mute all of them. + + Note: When tone starts in the middle of a Goertzel block, it won't be properly + detected in that block, only in the next. If we only mute the next block + where tone is actually detected, the user will still hear beginning + of the tone in preceeding block. This is why we usually want to mute some amount + of samples preceeding and following the block where tone was detected. +*/ + struct ast_dsp { struct ast_frame f; int threshold; @@ -336,13 +359,25 @@ int tcount; int digitmode; int faxmode; - int thinkdigit; + int dtmf_began; float genergy; + int mute_fragments; + fragment_t mute_data[5]; digit_detect_state_t digit_state; tone_detect_state_t cng_tone_state; tone_detect_state_t ced_tone_state; }; +static void mute_fragment(struct ast_dsp *dsp, fragment_t *fragment) +{ + if (dsp->mute_fragments >= sizeof(dsp->mute_data) / sizeof(dsp->mute_data[0])) { + ast_log(LOG_ERROR, "Too many fragments to mute. Ignoring\n"); + return; + } + + dsp->mute_data[dsp->mute_fragments++] = *fragment; +} + static void ast_tone_detect_init(tone_detect_state_t *s, int freq, int duration, int amp) { int duration_samples; @@ -416,8 +451,8 @@ s->lasthit = 0; s->current_hit = 0; for (i = 0; i < 4; i++) { - goertzel_init (&s->row_out[i], dtmf_row[i], 102); - goertzel_init (&s->col_out[i], dtmf_col[i], 102); + goertzel_init (&s->row_out[i], dtmf_row[i], DTMF_GSIZE); + goertzel_init (&s->col_out[i], dtmf_col[i], DTMF_GSIZE); s->energy = 0.0; } s->current_sample = 0; @@ -447,8 +482,7 @@ ast_dtmf_detect_init(&s->td.dtmf); } -static int tone_detect(tone_detect_state_t *s, int16_t *amp, int samples, - int *writeback) +static int tone_detect(struct ast_dsp *dsp, tone_detect_state_t *s, int16_t *amp, int samples) { float tone_energy; int i; @@ -456,10 +490,20 @@ int limit; int res = 0; int16_t *ptr; + int start, end; + fragment_t mute = {0, 0}; - while (1) { + if (s->squelch && s->mute_samples > 0) { + mute.end = (s->mute_samples < samples) ? s->mute_samples : samples; + s->mute_samples -= mute.end; + } + + for (start = 0; start < samples; start = end) { /* Process in blocks. */ - limit = (samples < s->samples_pending) ? samples : s->samples_pending; + limit = samples - start; + if (limit > s->samples_pending) + limit = s->samples_pending; + end = start + limit; for (i = limit, ptr = amp ; i > 0; i--, ptr++) { /* signed 32 bit int should be enough to suqare any possible signed 16 bit value */ @@ -472,17 +516,9 @@ if (s->samples_pending) { /* Finished incomplete (last) block */ - if (s->last_hit && s->squelch) { - /* If we had a hit last time, go ahead and clear this out since likely it - will be another hit */ - memset(amp, 0, sizeof(*amp) * limit); - if (writeback) - *writeback = 1; - } break; } - tone_energy = goertzel_result(&s->tone); /* Scale to make comparable */ @@ -495,13 +531,6 @@ ast_debug(10, "Hit! count=%d\n", s->hit_count); hit = 1; - - if (s->squelch) { - /* Zero out frame data */ - memset(amp, 0, sizeof(*amp) * limit); - if (writeback) - *writeback = 1; - } } if (s->hit_count) @@ -524,6 +553,17 @@ s->last_hit = hit; + /* If we had a hit in this block, include it into mute fragment */ + if (s->squelch && hit) { + if (mute.end < start - s->block_size) { + /* There is a gap between fragments */ + mute_fragment(dsp, &mute); + mute.start = (start > s->block_size) ? (start - s->block_size) : 0; + } + mute.end = end + s->block_size; + } + + /* Reinitialise the detector for the next block */ /* Reset for the next block */ goertzel_reset(&s->tone); @@ -532,7 +572,14 @@ s->samples_pending = s->block_size; amp += limit; - samples -= limit; + } + + if (s->squelch && mute.end) { + if (mute.end > samples) { + s->mute_samples = mute.end - samples; + mute.end = samples; + } + mute_fragment(dsp, &mute); } return res; @@ -550,8 +597,7 @@ } } -static int dtmf_detect(digit_detect_state_t *s, int16_t amp[], int samples, - int digitmode, int *writeback) +static int dtmf_detect(struct ast_dsp *dsp, digit_detect_state_t *s, int16_t amp[], int samples, int squelch, int relax) { float row_energy[4]; float col_energy[4]; @@ -563,12 +609,18 @@ int best_col; int hit; int limit; + fragment_t mute = {0, 0}; + + if (squelch && s->td.dtmf.mute_samples > 0) { + mute.end = (s->td.dtmf.mute_samples < samples) ? s->td.dtmf.mute_samples : samples; + s->td.dtmf.mute_samples -= mute.end; + } hit = 0; for (sample = 0; sample < samples; sample = limit) { - /* 102 is optimised to meet the DTMF specs. */ - if ((samples - sample) >= (102 - s->td.dtmf.current_sample)) - limit = sample + (102 - s->td.dtmf.current_sample); + /* DTMF_GSIZE is optimised to meet the DTMF specs. */ + if ((samples - sample) >= (DTMF_GSIZE - s->td.dtmf.current_sample)) + limit = sample + (DTMF_GSIZE - s->td.dtmf.current_sample); else limit = samples; /* The following unrolled loop takes only 35% (rough estimate) of the @@ -588,14 +640,7 @@ goertzel_sample(s->td.dtmf.col_out + 3, amp[j]); } s->td.dtmf.current_sample += (limit - sample); - if (s->td.dtmf.current_sample < 102) { - if (hit && !((digitmode & DSP_DIGITMODE_NOQUELCH))) { - /* If we had a hit last time, go ahead and clear this out since likely it - will be another hit */ - for (i=sample;itd.dtmf.current_sample < DTMF_GSIZE) { continue; } /* We are at the end of a DTMF detection block */ @@ -631,12 +676,6 @@ (row_energy[best_row] + col_energy[best_col]) > DTMF_TO_TOTAL_ENERGY*s->td.dtmf.energy) { /* Got a hit */ hit = dtmf_positions[(best_row << 2) + best_col]; - if (!(digitmode & DSP_DIGITMODE_NOQUELCH)) { - /* Zero out frame data if this is part DTMF */ - for (i=sample;itd.dtmf.lasthit = hit; + /* If we had a hit in this block, include it into mute fragment */ + if (squelch && hit) { + if (mute.end < sample - DTMF_GSIZE) { + /* There is a gap between fragments */ + mute_fragment(dsp, &mute); + mute.start = (sample > DTMF_GSIZE) ? (sample - DTMF_GSIZE) : 0; + } + mute.end = limit + DTMF_GSIZE; + } + /* Reinitialise the detector for the next block */ for (i = 0; i < 4; i++) { goertzel_reset(&s->td.dtmf.row_out[i]); @@ -664,14 +713,23 @@ s->td.dtmf.energy = 0.0; s->td.dtmf.current_sample = 0; } + + if (squelch && mute.end) { + if (mute.end > samples) { + s->td.dtmf.mute_samples = mute.end - samples; + mute.end = samples; + } + mute_fragment(dsp, &mute); + } + return (s->td.dtmf.current_hit); /* return the debounced hit */ } /* MF goertzel size */ #define MF_GSIZE 120 -static int mf_detect(digit_detect_state_t *s, int16_t amp[], - int samples, int digitmode, int *writeback) +static int mf_detect(struct ast_dsp *dsp, digit_detect_state_t *s, int16_t amp[], + int samples, int squelch, int relax) { float energy[6]; int best; @@ -682,6 +740,12 @@ int sample; int hit; int limit; + fragment_t mute = {0, 0}; + + if (squelch && s->td.mf.mute_samples > 0) { + mute.end = (s->td.mf.mute_samples < samples) ? s->td.mf.mute_samples : samples; + s->td.mf.mute_samples -= mute.end; + } hit = 0; for (sample = 0; sample < samples; sample = limit) { @@ -705,13 +769,6 @@ } s->td.mf.current_sample += (limit - sample); if (s->td.mf.current_sample < MF_GSIZE) { - if (hit && !((digitmode & DSP_DIGITMODE_NOQUELCH))) { - /* If we had a hit last time, go ahead and clear this out since likely it - will be another hit */ - for (i=sample;itd.mf.hits[2] = s->td.mf.hits[3]; s->td.mf.hits[3] = s->td.mf.hits[4]; s->td.mf.hits[4] = hit; + + /* If we had a hit in this block, include it into mute fragment */ + if (squelch && hit) { + if (mute.end < sample - MF_GSIZE) { + /* There is a gap between fragments */ + mute_fragment(dsp, &mute); + mute.start = (sample > MF_GSIZE) ? (sample - MF_GSIZE) : 0; + } + mute.end = limit + DTMF_GSIZE; + } + /* Reinitialise the detector for the next block */ for (i = 0; i < 6; i++) goertzel_reset(&s->td.mf.tone_out[i]); s->td.mf.current_sample = 0; } - return (s->td.mf.current_hit); /* return the debounced hit */ -} - -static int __ast_dsp_digitdetect(struct ast_dsp *dsp, short *s, int len, int *writeback) -{ - int res = 0; - - if ((dsp->features & DSP_FEATURE_DTMF_DETECT) && (dsp->digitmode & DSP_DIGITMODE_MF)) - res = mf_detect(&dsp->digit_state, s, len, dsp->digitmode & DSP_DIGITMODE_RELAXDTMF, writeback); - else if (dsp->features & DSP_FEATURE_DTMF_DETECT) - res = dtmf_detect(&dsp->digit_state, s, len, dsp->digitmode & DSP_DIGITMODE_RELAXDTMF, writeback); - - if ((dsp->features & DSP_FEATURE_FAX_DETECT) && (dsp->faxmode & DSP_FAXMODE_DETECT_CNG)) { - if (tone_detect(&dsp->cng_tone_state, s, len, NULL)) { - store_digit(&dsp->digit_state, 'f'); - res = 'f'; + if (squelch && mute.end) { + if (mute.end > samples) { + s->td.mf.mute_samples = mute.end - samples; + mute.end = samples; } + mute_fragment(dsp, &mute); } - if ((dsp->features & DSP_FEATURE_FAX_DETECT) && (dsp->faxmode & DSP_FAXMODE_DETECT_CED)) { - if (tone_detect(&dsp->ced_tone_state, s, len, NULL)) { - store_digit(&dsp->digit_state, 'e'); - res = 'e'; - } - } - - return res; -} - -int ast_dsp_digitdetect(struct ast_dsp *dsp, struct ast_frame *inf) -{ - short *s; - int len; - int ign=0; - - if (inf->frametype != AST_FRAME_VOICE) { - ast_log(LOG_WARNING, "Can't check call progress of non-voice frames\n"); - return 0; - } - if (inf->subclass != AST_FORMAT_SLINEAR) { - ast_log(LOG_WARNING, "Can only check call progress in signed-linear frames\n"); - return 0; - } - s = inf->data; - len = inf->datalen / 2; - return __ast_dsp_digitdetect(dsp, s, len, &ign); + return (s->td.mf.current_hit); /* return the debounced hit */ } static inline int pair_there(float p1, float p2, float i1, float i2, float e) @@ -865,19 +896,6 @@ return 1; } -int ast_dsp_getdigits(struct ast_dsp *dsp, char *buf, int max) -{ - if (max > dsp->digit_state.current_digits) - max = dsp->digit_state.current_digits; - if (max > 0) { - memcpy(buf, dsp->digit_state.digits, max); - memmove(dsp->digit_state.digits, dsp->digit_state.digits + max, dsp->digit_state.current_digits - max); - dsp->digit_state.current_digits -= max; - } - buf[max] = '\0'; - return max; -} - static int __ast_dsp_call_progress(struct ast_dsp *dsp, short *s, int len) { int x; @@ -1181,34 +1199,18 @@ { int silence; int res; - int digit; + int digit = 0, fax_digit = 0; int x; short *shortdata; unsigned char *odata; int len; - int writeback = 0; - -#define FIX_INF(inf) do { \ - if (writeback) { \ - switch (inf->subclass) { \ - case AST_FORMAT_SLINEAR: \ - break; \ - case AST_FORMAT_ULAW: \ - for (x=0;xframetype != AST_FRAME_VOICE) return af; + odata = af->data; len = af->datalen; /* Make sure we have short data */ @@ -1231,6 +1233,10 @@ ast_log(LOG_WARNING, "Inband DTMF is not supported on codec %s. Use RFC2833\n", ast_getformatname(af->subclass)); return af; } + + /* Initially we do not want to mute anything */ + dsp->mute_fragments = 0; + silence = __ast_dsp_silence(dsp, shortdata, len, NULL); if ((dsp->features & DSP_FEATURE_SILENCE_SUPPRESS) && silence) { memset(&dsp->f, 0, sizeof(dsp->f)); @@ -1247,88 +1253,68 @@ ast_debug(1, "Requesting Hangup because the busy tone was detected on channel %s\n", chan->name); return &dsp->f; } - if (((dsp->features & DSP_FEATURE_DTMF_DETECT) || (dsp->features & DSP_FEATURE_FAX_DETECT))) { - digit = __ast_dsp_digitdetect(dsp, shortdata, len, &writeback); -#if 0 - if (digit) - printf("Performing digit detection returned %d, digitmode is %d\n", digit, dsp->digitmode); -#endif - if (dsp->digitmode & (DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX)) { - if (!dsp->thinkdigit) { - if (digit) { - /* Looks like we might have something. - * Request a conference mute for the moment */ - memset(&dsp->f, 0, sizeof(dsp->f)); - dsp->f.frametype = AST_FRAME_DTMF; - dsp->f.subclass = 'm'; - dsp->thinkdigit = 'x'; - FIX_INF(af); - if (chan) - ast_queue_frame(chan, af); - ast_frfree(af); - return &dsp->f; - } - } else { - if (digit) { - /* Thought we saw one last time. Pretty sure we really have now */ - if ((dsp->thinkdigit != 'x') && (dsp->thinkdigit != digit)) { - /* If we found a digit, and we're changing digits, go - ahead and send this one, but DON'T stop confmute because - we're detecting something else, too... */ - memset(&dsp->f, 0, sizeof(dsp->f)); - dsp->f.frametype = AST_FRAME_DTMF_END; - dsp->f.subclass = dsp->thinkdigit; - FIX_INF(af); - if (chan) - ast_queue_frame(chan, af); - ast_frfree(af); - } else { - dsp->thinkdigit = digit; - memset(&dsp->f, 0, sizeof(dsp->f)); - dsp->f.frametype = AST_FRAME_DTMF_BEGIN; - dsp->f.subclass = dsp->thinkdigit; - FIX_INF(af); - if (chan) - ast_queue_frame(chan, af); - ast_frfree(af); - } - return &dsp->f; - } else { - memset(&dsp->f, 0, sizeof(dsp->f)); - if (dsp->thinkdigit != 'x') { - /* If we found a digit, send it now */ - dsp->f.frametype = AST_FRAME_DTMF_END; - dsp->f.subclass = dsp->thinkdigit; - dsp->thinkdigit = 0; - } else { - dsp->f.frametype = AST_FRAME_DTMF; - dsp->f.subclass = 'u'; - dsp->thinkdigit = 0; - } - FIX_INF(af); - if (chan) - ast_queue_frame(chan, af); - ast_frfree(af); - return &dsp->f; - } + + /* Do the fax detection before DTMF to let detector see the signal which may be squelched + by DTMF detetor later */ + if ((dsp->features & DSP_FEATURE_FAX_DETECT)) { + + if ((dsp->faxmode & DSP_FAXMODE_DETECT_CNG) && tone_detect(dsp, &dsp->cng_tone_state, shortdata, len)) { + fax_digit = 'f'; + } + + if ((dsp->faxmode & DSP_FAXMODE_DETECT_CED) && tone_detect(dsp, &dsp->ced_tone_state, shortdata, len)) { + fax_digit = 'e'; + } + } + + if ((dsp->features & DSP_FEATURE_DTMF_DETECT)) { + + if ((dsp->features & DSP_FEATURE_DTMF_DETECT) && (dsp->digitmode & DSP_DIGITMODE_MF)) + digit = mf_detect(dsp, &dsp->digit_state, shortdata, len, (dsp->digitmode & DSP_DIGITMODE_NOQUELCH) == 0, dsp->digitmode & DSP_DIGITMODE_RELAXDTMF); + else if (dsp->features & DSP_FEATURE_DTMF_DETECT) + digit = dtmf_detect(dsp, &dsp->digit_state, shortdata, len, (dsp->digitmode & DSP_DIGITMODE_NOQUELCH) == 0, dsp->digitmode & DSP_DIGITMODE_RELAXDTMF); + + if (dsp->digit_state.current_digits) { + int event = 0; + char event_digit = 0; + + if (!dsp->dtmf_began) { + /* We have not reported DTMF_BEGIN for anything yet */ + + event = AST_FRAME_DTMF_BEGIN; + event_digit = dsp->digit_state.digits[0]; + dsp->dtmf_began = 1; + + } else if (dsp->digit_state.current_digits > 1 || digit != dsp->digit_state.digits[0]) { + /* Digit changed. This means digit we have reported with DTMF_BEGIN ended */ + + event = AST_FRAME_DTMF_END; + event_digit = dsp->digit_state.digits[0]; + memmove(dsp->digit_state.digits, dsp->digit_state.digits + 1, dsp->digit_state.current_digits); + dsp->digit_state.current_digits--; + dsp->dtmf_began = 0; + } + + if (event) { + memset(&dsp->f, 0, sizeof(dsp->f)); + dsp->f.frametype = event; + dsp->f.subclass = event_digit; + outf = &dsp->f; + goto done; } - } else if (dsp->digit_state.current_digits > 1 || - (dsp->digit_state.current_digits == 1 && digit != dsp->digit_state.digits[0])) { - /* Since we basically generate DTMF_END frames we do it only when a digit - has finished. */ - - memset(&dsp->f, 0, sizeof(dsp->f)); - dsp->f.frametype = AST_FRAME_DTMF; - dsp->f.subclass = dsp->digit_state.digits[0]; - memmove(dsp->digit_state.digits, dsp->digit_state.digits + 1, dsp->digit_state.current_digits); - dsp->digit_state.current_digits--; - FIX_INF(af); - if (chan) - ast_queue_frame(chan, af); - ast_frfree(af); - return &dsp->f; } } + + if (fax_digit) { + /* Fax was detected - digit is either 'f' or 'e' */ + + memset(&dsp->f, 0, sizeof(dsp->f)); + dsp->f.frametype = AST_FRAME_DTMF; + dsp->f.subclass = fax_digit; + outf = &dsp->f; + goto done; + } + if ((dsp->features & DSP_FEATURE_CALL_PROGRESS)) { res = __ast_dsp_call_progress(dsp, shortdata, len); if (res) { @@ -1350,8 +1336,34 @@ } } } - FIX_INF(af); - return af; + +done: + /* Mute fragment of the frame */ + for (x = 0; x < dsp->mute_fragments; x++) { + memset(shortdata + dsp->mute_data[x].start, 0, sizeof(int16_t) * (dsp->mute_data[x].end - dsp->mute_data[x].start)); + } + + switch (af->subclass) { + case AST_FORMAT_SLINEAR: + break; + case AST_FORMAT_ULAW: + for (x = 0; x < len; x++) + odata[x] = AST_LIN2MU((unsigned short) shortdata[x]); + break; + case AST_FORMAT_ALAW: + for (x = 0; x < len; x++) + odata[x] = AST_LIN2A((unsigned short) shortdata[x]); + break; + } + + if (outf) { + if (chan) + ast_queue_frame(chan, af); + ast_frfree(af); + return outf; + } else { + return af; + } } static void ast_dsp_prog_reset(struct ast_dsp *dsp) @@ -1426,7 +1438,7 @@ { int i; - dsp->thinkdigit = 0; + dsp->dtmf_began = 0; if (dsp->digitmode & DSP_DIGITMODE_MF) { mf_detect_state_t *s = &dsp->digit_state.td.mf; /* Reinitialise the detector for the next block */