dsp.c

/*
 * Asterisk -- A telephony toolkit for Linux.
 *
 * Convenience Signal Processing routines
 * 
 * Copyright (C) 2002, Digium
 *
 * Mark Spencer <markster@linux-support.net>
 *
 * This program is free software, distributed under the terms of
 * the GNU General Public License.
 *
 * Goertzel routines are borrowed from Steve Underwood's tremendous work on the
 * DTMF detector.
 *
 */

/* Some routines from tone_detect.c by Steven Underwood as published under the zapata library */
/*
	tone_detect.c - General telephony tone detection, and specific
                        detection of DTMF.

        Copyright (C) 2001  Steve Underwood <steveu@coppice.org>

        Despite my general liking of the GPL, I place this code in the
        public domain for the benefit of all mankind - even the slimy
        ones who might try to proprietize my work and use it to my
        detriment.
*/

#include <asterisk/frame.h>
#include <asterisk/channel.h>
#include <asterisk/channel_pvt.h>
#include <asterisk/logger.h>
#include <asterisk/dsp.h>
#include <asterisk/ulaw.h>
#include <asterisk/alaw.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <math.h>
#include <errno.h>
#include <stdio.h>

#define DEFAULT_THRESHOLD 1024

#define BUSY_THRESHOLD	100		/* Max number of ms difference between max and min times in busy */
#define BUSY_MIN		80		/* Busy must be at least 80 ms in half-cadence */
#define BUSY_MAX		1100	/* Busy can't be longer than 1100 ms in half-cadence */

/* Remember last 3 units */
#define DSP_HISTORY 5

/* Number of goertzels for progress detect */
#define GSAMP_SIZE 183

#define HZ_350  0
#define HZ_440  1
#define HZ_480  2
#define HZ_620  3
#define HZ_950  4
#define HZ_1400 5
#define HZ_1800 6

#define TONE_THRESH 10.0	/* How much louder the tone should be than channel energy */
#define TONE_MIN_THRESH 1e8	/* How much tone there should be at least to attempt */
#define COUNT_THRESH  3		/* Need at least 50ms of stuff to count it */

#define TONE_STATE_SILENCE  0
#define TONE_STATE_RINGING  1 
#define TONE_STATE_DIALTONE 2
#define TONE_STATE_TALKING  3
#define TONE_STATE_BUSY     4
#define TONE_STATE_SPECIAL1	5
#define TONE_STATE_SPECIAL2 6
#define TONE_STATE_SPECIAL3 7

#define	MAX_DTMF_DIGITS 128

/* Basic DTMF specs:
 *
 * Minimum tone on = 40ms
 * Minimum tone off = 50ms
 * Maximum digit rate = 10 per second
 * Normal twist <= 8dB accepted
 * Reverse twist <= 4dB accepted
 * S/N >= 15dB will detect OK
 * Attenuation <= 26dB will detect OK
 * Frequency tolerance +- 1.5% will detect, +-3.5% will reject
 */

#define DTMF_THRESHOLD              8.0e7
#define FAX_THRESHOLD              8.0e7
#define FAX_2ND_HARMONIC       		2.0     /* 4dB */
#define DTMF_NORMAL_TWIST           6.3     /* 8dB */
#define DTMF_REVERSE_TWIST          ((digitmode & DSP_DIGITMODE_RELAXDTMF) ? 4.0 : 2.5)     /* 4dB normal */
#define DTMF_RELATIVE_PEAK_ROW      6.3     /* 8dB */
#define DTMF_RELATIVE_PEAK_COL      6.3     /* 8dB */
#define DTMF_2ND_HARMONIC_ROW       ((digitmode & DSP_DIGITMODE_RELAXDTMF) ? 1.7 : 2.5)     /* 4dB normal */
#define DTMF_2ND_HARMONIC_COL       63.1    /* 18dB */

#define MF_THRESHOLD              8.0e7
#define MF_NORMAL_TWIST           5.3     /* 8dB */
#define MF_REVERSE_TWIST          4.0     /* was 2.5 */
#define MF_RELATIVE_PEAK      5.3     /* 8dB */
#define MF_2ND_HARMONIC       1.7 /* was 2.5  */

typedef struct {
	float v2;
	float v3;
	float fac;
} goertzel_state_t;

typedef struct
{
    int hit1;
    int hit2;
    int hit3;
    int hit4;
    int mhit;

    goertzel_state_t row_out[4];
    goertzel_state_t col_out[4];
    goertzel_state_t row_out2nd[4];
    goertzel_state_t col_out2nd[4];
	goertzel_state_t fax_tone;
	goertzel_state_t fax_tone2nd;
    float energy;
    
    int current_sample;
    char digits[MAX_DTMF_DIGITS + 1];
    int current_digits;
    int detected_digits;
    int lost_digits;
    int digit_hits[16];
	int fax_hits;
} dtmf_detect_state_t;

typedef struct
{
    int hit1;
    int hit2;
    int hit3;
    int hit4;
    int mhit;

    goertzel_state_t tone_out[6];
    goertzel_state_t tone_out2nd[6];
    float energy;
    
    int current_sample;
    char digits[MAX_DTMF_DIGITS + 1];
    int current_digits;
    int detected_digits;
    int lost_digits;
	int fax_hits;
} mf_detect_state_t;

static float dtmf_row[] =
{
     697.0,  770.0,  852.0,  941.0
};
static float dtmf_col[] =
{
    1209.0, 1336.0, 1477.0, 1633.0
};

static float mf_tones[] =
{
	700.0, 900.0, 1100.0, 1300.0, 1500.0, 1700.0
};

static float fax_freq = 1100.0;

static char dtmf_positions[] = "123A" "456B" "789C" "*0#D";

static char mf_hit[6][6] = {
	/*  700 + */ {   0, '1', '2', '4', '7', 'C' },
	/*  900 + */ { '1',   0, '3', '5', '8', 'A' },
	/* 1100 + */ { '2', '3',   0, '6', '9', '*' },
	/* 1300 + */ { '4', '5', '6',   0, '0', 'B' },
	/* 1500 + */ { '7', '8', '9', '0',  0, '#' },
	/* 1700 + */ { 'C', 'A', '*', 'B', '#',  0  },
};

static inline void goertzel_sample(goertzel_state_t *s, short sample)
{
	float v1;
	float fsamp  = sample;
	v1 = s->v2;
	s->v2 = s->v3;
	s->v3 = s->fac * s->v2 - v1 + fsamp;
}

static inline void goertzel_update(goertzel_state_t *s, short *samps, int count)
{
	int i;
	for (i=0;i<count;i++) 
		goertzel_sample(s, samps[i]);
}


static inline float goertzel_result(goertzel_state_t *s)
{
	return s->v3 * s->v3 + s->v2 * s->v2 - s->v2 * s->v3 * s->fac;
}

static inline void goertzel_init(goertzel_state_t *s, float freq)
{
	s->v2 = s->v3 = 0.0;
	s->fac = 2.0 * cos(2.0 * M_PI * (freq / 8000.0));
}

static inline void goertzel_reset(goertzel_state_t *s)
{
	s->v2 = s->v3 = 0.0;
}

struct ast_dsp {
	struct ast_frame f;
	int threshold;
	int totalsilence;
	int totalnoise;
	int features;
	int busymaybe;
	int busycount;
	int historicnoise[DSP_HISTORY];
	int historicsilence[DSP_HISTORY];
	goertzel_state_t freqs[7];
	int gsamps;
	int tstate;
	int tcount;
	int digitmode;
	int thinkdigit;
	float genergy;
	union {
		dtmf_detect_state_t dtmf;
		mf_detect_state_t mf;
	} td;
};

static void ast_dtmf_detect_init (dtmf_detect_state_t *s)
{
    int i;

    s->hit1 = 
    s->hit2 = 0;

    for (i = 0;  i < 4;  i++)
    {
    
   		goertzel_init (&s->row_out[i], dtmf_row[i]);
    	goertzel_init (&s->col_out[i], dtmf_col[i]);
    	goertzel_init (&s->row_out2nd[i], dtmf_row[i] * 2.0);
    	goertzel_init (&s->col_out2nd[i], dtmf_col[i] * 2.0);
	
		s->energy = 0.0;
    }

	/* Same for the fax dector */
    goertzel_init (&s->fax_tone, fax_freq);

	/* Same for the fax dector 2nd harmonic */
    goertzel_init (&s->fax_tone2nd, fax_freq * 2.0);
	
    s->current_sample = 0;
    s->detected_digits = 0;
	s->current_digits = 0;
	memset(&s->digits, 0, sizeof(s->digits));
    s->lost_digits = 0;
    s->digits[0] = '\0';
    s->mhit = 0;
}

static void ast_mf_detect_init (mf_detect_state_t *s)
{
    int i;

    s->hit1 = 
    s->hit2 = 0;

    for (i = 0;  i < 6;  i++)
    {
    
   		goertzel_init (&s->tone_out[i], mf_tones[i]);
    	goertzel_init (&s->tone_out2nd[i], mf_tones[i] * 2.0);
	
		s->energy = 0.0;
    }

	s->current_digits = 0;
	memset(&s->digits, 0, sizeof(s->digits));
    s->current_sample = 0;
    s->detected_digits = 0;
    s->lost_digits = 0;
    s->digits[0] = '\0';
    s->mhit = 0;
}

static int dtmf_detect (dtmf_detect_state_t *s,
                 int16_t amp[],
                 int samples, 
		 int digitmode, int *writeback)
{

    float row_energy[4];
    float col_energy[4];
    float fax_energy;
    float fax_energy_2nd;
    float famp;
    float v1;
    int i;
    int j;
    int sample;
    int best_row;
    int best_col;
    int hit;
    int limit;

    hit = 0;
    for (sample = 0;  sample < samples;  sample = limit)
    {
        /* 102 is optimised to meet the DTMF specs. */
        if ((samples - sample) >= (102 - s->current_sample))
            limit = sample + (102 - s->current_sample);
        else
            limit = samples;
#if defined(USE_3DNOW)
        _dtmf_goertzel_update (s->row_out, amp + sample, limit - sample);
        _dtmf_goertzel_update (s->col_out, amp + sample, limit - sample);
        _dtmf_goertzel_update (s->row_out2nd, amp + sample, limit2 - sample);
        _dtmf_goertzel_update (s->col_out2nd, amp + sample, limit2 - sample);
		/* XXX Need to fax detect for 3dnow too XXX */
		#warning "Fax Support Broken"
#else
        /* The following unrolled loop takes only 35% (rough estimate) of the 
           time of a rolled loop on the machine on which it was developed */
        for (j = sample;  j < limit;  j++)
        {
            famp = amp[j];
	    
	    s->energy += famp*famp;
	    
            /* With GCC 2.95, the following unrolled code seems to take about 35%
               (rough estimate) as long as a neat little 0-3 loop */
            v1 = s->row_out[0].v2;
            s->row_out[0].v2 = s->row_out[0].v3;
            s->row_out[0].v3 = s->row_out[0].fac*s->row_out[0].v2 - v1 + famp;
    
            v1 = s->col_out[0].v2;
            s->col_out[0].v2 = s->col_out[0].v3;
            s->col_out[0].v3 = s->col_out[0].fac*s->col_out[0].v2 - v1 + famp;
    
            v1 = s->row_out[1].v2;
            s->row_out[1].v2 = s->row_out[1].v3;
            s->row_out[1].v3 = s->row_out[1].fac*s->row_out[1].v2 - v1 + famp;
    
            v1 = s->col_out[1].v2;
            s->col_out[1].v2 = s->col_out[1].v3;
            s->col_out[1].v3 = s->col_out[1].fac*s->col_out[1].v2 - v1 + famp;
    
            v1 = s->row_out[2].v2;
            s->row_out[2].v2 = s->row_out[2].v3;
            s->row_out[2].v3 = s->row_out[2].fac*s->row_out[2].v2 - v1 + famp;
    
            v1 = s->col_out[2].v2;
            s->col_out[2].v2 = s->col_out[2].v3;
            s->col_out[2].v3 = s->col_out[2].fac*s->col_out[2].v2 - v1 + famp;
    
            v1 = s->row_out[3].v2;
            s->row_out[3].v2 = s->row_out[3].v3;
            s->row_out[3].v3 = s->row_out[3].fac*s->row_out[3].v2 - v1 + famp;

            v1 = s->col_out[3].v2;
            s->col_out[3].v2 = s->col_out[3].v3;
            s->col_out[3].v3 = s->col_out[3].fac*s->col_out[3].v2 - v1 + famp;

            v1 = s->col_out2nd[0].v2;
            s->col_out2nd[0].v2 = s->col_out2nd[0].v3;
            s->col_out2nd[0].v3 = s->col_out2nd[0].fac*s->col_out2nd[0].v2 - v1 + famp;
        
            v1 = s->row_out2nd[0].v2;
            s->row_out2nd[0].v2 = s->row_out2nd[0].v3;
            s->row_out2nd[0].v3 = s->row_out2nd[0].fac*s->row_out2nd[0].v2 - v1 + famp;
        
            v1 = s->col_out2nd[1].v2;
            s->col_out2nd[1].v2 = s->col_out2nd[1].v3;
            s->col_out2nd[1].v3 = s->col_out2nd[1].fac*s->col_out2nd[1].v2 - v1 + famp;
    
            v1 = s->row_out2nd[1].v2;
            s->row_out2nd[1].v2 = s->row_out2nd[1].v3;
            s->row_out2nd[1].v3 = s->row_out2nd[1].fac*s->row_out2nd[1].v2 - v1 + famp;
        
            v1 = s->col_out2nd[2].v2;
            s->col_out2nd[2].v2 = s->col_out2nd[2].v3;
            s->col_out2nd[2].v3 = s->col_out2nd[2].fac*s->col_out2nd[2].v2 - v1 + famp;
        
            v1 = s->row_out2nd[2].v2;
            s->row_out2nd[2].v2 = s->row_out2nd[2].v3;
            s->row_out2nd[2].v3 = s->row_out2nd[2].fac*s->row_out2nd[2].v2 - v1 + famp;
        
            v1 = s->col_out2nd[3].v2;
            s->col_out2nd[3].v2 = s->col_out2nd[3].v3;
            s->col_out2nd[3].v3 = s->col_out2nd[3].fac*s->col_out2nd[3].v2 - v1 + famp;
        
            v1 = s->row_out2nd[3].v2;
            s->row_out2nd[3].v2 = s->row_out2nd[3].v3;
            s->row_out2nd[3].v3 = s->row_out2nd[3].fac*s->row_out2nd[3].v2 - v1 + famp;

			/* Update fax tone */
            v1 = s->fax_tone.v2;
            s->fax_tone.v2 = s->fax_tone.v3;
            s->fax_tone.v3 = s->fax_tone.fac*s->fax_tone.v2 - v1 + famp;

            v1 = s->fax_tone.v2;
            s->fax_tone2nd.v2 = s->fax_tone2nd.v3;
            s->fax_tone2nd.v3 = s->fax_tone2nd.fac*s->fax_tone2nd.v2 - v1 + famp;
        }
#endif
        s->current_sample += (limit - sample);
        if (s->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;i<limit;i++) 
					amp[i] = 0;
				*writeback = 1;
			}
            continue;
		}

		/* Detect the fax energy, too */
		fax_energy = goertzel_result(&s->fax_tone);
		
        /* We are at the end of a DTMF detection block */
        /* Find the peak row and the peak column */
        row_energy[0] = goertzel_result (&s->row_out[0]);
        col_energy[0] = goertzel_result (&s->col_out[0]);

	for (best_row = best_col = 0, i = 1;  i < 4;  i++)
	{
    	    row_energy[i] = goertzel_result (&s->row_out[i]);
            if (row_energy[i] > row_energy[best_row])
                best_row = i;
    	    col_energy[i] = goertzel_result (&s->col_out[i]);
            if (col_energy[i] > col_energy[best_col])
                best_col = i;
    	}
        hit = 0;
        /* Basic signal level test and the twist test */
        if (row_energy[best_row] >= DTMF_THRESHOLD
	    &&
	    col_energy[best_col] >= DTMF_THRESHOLD
            &&
            col_energy[best_col] < row_energy[best_row]*DTMF_REVERSE_TWIST
            &&
            col_energy[best_col]*DTMF_NORMAL_TWIST > row_energy[best_row])
        {
            /* Relative peak test */
            for (i = 0;  i < 4;  i++)
            {
                if ((i != best_col  &&  col_energy[i]*DTMF_RELATIVE_PEAK_COL > col_energy[best_col])
                    ||
                    (i != best_row  &&  row_energy[i]*DTMF_RELATIVE_PEAK_ROW > row_energy[best_row]))
                {
                    break;
                }
            }
            /* ... and second harmonic test */
            if (i >= 4
	        &&
		(row_energy[best_row] + col_energy[best_col]) > 42.0*s->energy
                &&
                goertzel_result (&s->col_out2nd[best_col])*DTMF_2ND_HARMONIC_COL < col_energy[best_col]
                &&
                goertzel_result (&s->row_out2nd[best_row])*DTMF_2ND_HARMONIC_ROW < row_energy[best_row])
            {
				/* 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;i<limit;i++) 
						amp[i] = 0;
					*writeback = 1;
				}
                /* Look for two successive similar results */
                /* The logic in the next test is:
                   We need two successive identical clean detects, with
		   something different preceeding it. This can work with
		   back to back differing digits. More importantly, it
		   can work with nasty phones that give a very wobbly start
		   to a digit. */
                if (hit == s->hit3  &&  s->hit3 != s->hit2)
                {
		    s->mhit = hit;
                    s->digit_hits[(best_row << 2) + best_col]++;
                    s->detected_digits++;
                    if (s->current_digits < MAX_DTMF_DIGITS)
                    {
                        s->digits[s->current_digits++] = hit;
                        s->digits[s->current_digits] = '\0';
                    }
                    else
                    {
                        s->lost_digits++;
                    }
                }
            }
        } 
		if (!hit && (fax_energy >= FAX_THRESHOLD) && (fax_energy > s->energy * 21.0)) {
				fax_energy_2nd = goertzel_result(&s->fax_tone2nd);
				if (fax_energy_2nd * FAX_2ND_HARMONIC < fax_energy) {
#if 0
					printf("Fax energy/Second Harmonic: %f/%f\n", fax_energy, fax_energy_2nd);
#endif					
					/* XXX Probably need better checking than just this the energy XXX */
					hit = 'f';
					s->fax_hits++;
				} /* Don't reset fax hits counter */
		} else {
			if (s->fax_hits > 5) {
				 hit = 'f';
				 s->mhit = 'f';
	             s->detected_digits++;
	             if (s->current_digits < MAX_DTMF_DIGITS)
	             {
	                  s->digits[s->current_digits++] = hit;
	                  s->digits[s->current_digits] = '\0';
	             }
	             else
	             {
	                   s->lost_digits++;
	             }
			}
			s->fax_hits = 0;
		}
        s->hit1 = s->hit2;
        s->hit2 = s->hit3;
        s->hit3 = hit;
        /* Reinitialise the detector for the next block */
        for (i = 0;  i < 4;  i++)
        {
       	    goertzel_reset(&s->row_out[i]);
            goertzel_reset(&s->col_out[i]);
    	    goertzel_reset(&s->row_out2nd[i]);
    	    goertzel_reset(&s->col_out2nd[i]);
        }
    	goertzel_reset (&s->fax_tone);
    	goertzel_reset (&s->fax_tone2nd);
		s->energy = 0.0;
        s->current_sample = 0;
    }
    if ((!s->mhit) || (s->mhit != hit))
    {
	s->mhit = 0;
	return(0);
    }
    return (hit);
}

/* MF goertzel size */
#define	MF_GSIZE 160

static int mf_detect (mf_detect_state_t *s,
                 int16_t amp[],
                 int samples, 
		 int digitmode, int *writeback)
{

    float tone_energy[6];
    float famp;
    float v1;
    int i;
    int j;
    int sample;
    int best1;
    int best2;
	float max;
    int hit;
    int limit;
	int sofarsogood;

    hit = 0;
    for (sample = 0;  sample < samples;  sample = limit)
    {
        /* 80 is optimised to meet the MF specs. */
        if ((samples - sample) >= (MF_GSIZE - s->current_sample))
            limit = sample + (MF_GSIZE - s->current_sample);
        else
            limit = samples;
#if defined(USE_3DNOW)
        _dtmf_goertzel_update (s->row_out, amp + sample, limit - sample);
        _dtmf_goertzel_update (s->col_out, amp + sample, limit - sample);
        _dtmf_goertzel_update (s->row_out2nd, amp + sample, limit2 - sample);
        _dtmf_goertzel_update (s->col_out2nd, amp + sample, limit2 - sample);
		/* XXX Need to fax detect for 3dnow too XXX */
		#warning "Fax Support Broken"
#else
        /* The following unrolled loop takes only 35% (rough estimate) of the 
           time of a rolled loop on the machine on which it was developed */
        for (j = sample;  j < limit;  j++)
        {
            famp = amp[j];
	    
	    s->energy += famp*famp;
	    
            /* With GCC 2.95, the following unrolled code seems to take about 35%
               (rough estimate) as long as a neat little 0-3 loop */
            v1 = s->tone_out[0].v2;
            s->tone_out[0].v2 = s->tone_out[0].v3;
            s->tone_out[0].v3 = s->tone_out[0].fac*s->tone_out[0].v2 - v1 + famp;

            v1 = s->tone_out[1].v2;
            s->tone_out[1].v2 = s->tone_out[1].v3;
            s->tone_out[1].v3 = s->tone_out[1].fac*s->tone_out[1].v2 - v1 + famp;
    
            v1 = s->tone_out[2].v2;
            s->tone_out[2].v2 = s->tone_out[2].v3;
            s->tone_out[2].v3 = s->tone_out[2].fac*s->tone_out[2].v2 - v1 + famp;
    
            v1 = s->tone_out[3].v2;
            s->tone_out[3].v2 = s->tone_out[3].v3;
            s->tone_out[3].v3 = s->tone_out[3].fac*s->tone_out[3].v2 - v1 + famp;

            v1 = s->tone_out[4].v2;
            s->tone_out[4].v2 = s->tone_out[4].v3;
            s->tone_out[4].v3 = s->tone_out[4].fac*s->tone_out[4].v2 - v1 + famp;

            v1 = s->tone_out[5].v2;
            s->tone_out[5].v2 = s->tone_out[5].v3;
            s->tone_out[5].v3 = s->tone_out[5].fac*s->tone_out[5].v2 - v1 + famp;

            v1 = s->tone_out2nd[0].v2;
            s->tone_out2nd[0].v2 = s->tone_out2nd[0].v3;
            s->tone_out2nd[0].v3 = s->tone_out2nd[0].fac*s->tone_out2nd[0].v2 - v1 + famp;
        
            v1 = s->tone_out2nd[1].v2;
            s->tone_out2nd[1].v2 = s->tone_out2nd[1].v3;
            s->tone_out2nd[1].v3 = s->tone_out2nd[1].fac*s->tone_out2nd[1].v2 - v1 + famp;
        
            v1 = s->tone_out2nd[2].v2;
            s->tone_out2nd[2].v2 = s->tone_out2nd[2].v3;
            s->tone_out2nd[2].v3 = s->tone_out2nd[2].fac*s->tone_out2nd[2].v2 - v1 + famp;
        
            v1 = s->tone_out2nd[3].v2;
            s->tone_out2nd[3].v2 = s->tone_out2nd[3].v3;
            s->tone_out2nd[3].v3 = s->tone_out2nd[3].fac*s->tone_out2nd[3].v2 - v1 + famp;

            v1 = s->tone_out2nd[4].v2;
            s->tone_out2nd[4].v2 = s->tone_out2nd[4].v3;
            s->tone_out2nd[4].v3 = s->tone_out2nd[4].fac*s->tone_out2nd[2].v2 - v1 + famp;
        
            v1 = s->tone_out2nd[3].v2;
            s->tone_out2nd[5].v2 = s->tone_out2nd[6].v3;
            s->tone_out2nd[5].v3 = s->tone_out2nd[6].fac*s->tone_out2nd[3].v2 - v1 + famp;

        }
#endif
        s->current_sample += (limit - sample);
        if (s->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;i<limit;i++) 
					amp[i] = 0;
				*writeback = 1;
			}
            continue;
		}

		/* We're at the end of an MF detection block.  Go ahead and calculate
		   all the energies. */
		for (i=0;i<6;i++) {
			tone_energy[i] = goertzel_result(&s->tone_out[i]);
		}
		
		/* Find highest */
		best1 = 0;
		max = tone_energy[0];
		for (i=1;i<6;i++) {
			if (tone_energy[i] > max) {
				max = tone_energy[i];
				best1 = i;
			}
		}

		/* Find 2nd highest */
		if (best1)
			max = tone_energy[0];
		else
			max = tone_energy[1];
		best2 = 0;
		for (i=0;i<6;i++) {
			if (i == best1) continue;
			if (tone_energy[i] > max) {
				max = tone_energy[i];
				best2 = i;
			}
		}
		
        hit = 0;
		sofarsogood=1;
		/* Check for relative energies */
		for (i=0;i<6;i++) {
			if (i == best1) continue;
			if (i == best2) continue;
			if (tone_energy[best1] < tone_energy[i] * MF_RELATIVE_PEAK) {
				sofarsogood = 0;
				break;
			}
			if (tone_energy[best2] < tone_energy[i] * MF_RELATIVE_PEAK) {
				sofarsogood = 0;
				break;
			}
		}
		
		if (sofarsogood) {
			/* Check for 2nd harmonic */
			if (goertzel_result(&s->tone_out2nd[best1]) * MF_2ND_HARMONIC > tone_energy[best1]) 
				sofarsogood = 0;
			else if (goertzel_result(&s->tone_out2nd[best1]) * MF_2ND_HARMONIC > tone_energy[best2])
				sofarsogood = 0;
		}
		if (sofarsogood) {
			hit = mf_hit[best1][best2];
			if (!(digitmode & DSP_DIGITMODE_NOQUELCH)) {
				/* Zero out frame data if this is part DTMF */
				for (i=sample;i<limit;i++) 
					amp[i] = 0;
				*writeback = 1;
			}
			/* Look for two consecutive clean hits */
			if ((hit == s->hit3) && (s->hit3 != s->hit2)) {
				s->mhit = hit;
				s->detected_digits++;
				if (s->current_digits < MAX_DTMF_DIGITS - 2) {
					s->digits[s->current_digits++] = hit;
					s->digits[s->current_digits] = '\0';
				} else {
					s->lost_digits++;
				}
			}
		}
		
        s->hit1 = s->hit2;
        s->hit2 = s->hit3;
        s->hit3 = hit;
        /* Reinitialise the detector for the next block */
        for (i = 0;  i < 6;  i++)
        {
       	    goertzel_reset(&s->tone_out[i]);
            goertzel_reset(&s->tone_out2nd[i]);
        }
		s->energy = 0.0;
        s->current_sample = 0;
    }
    if ((!s->mhit) || (s->mhit != hit))
    {
		s->mhit = 0;
		return(0);
    }
    return (hit);
}

static int __ast_dsp_digitdetect(struct ast_dsp *dsp, short *s, int len, int *writeback)
{
	int res;
	if (dsp->digitmode & DSP_DIGITMODE_MF)
		res = mf_detect(&dsp->td.mf, s, len, dsp->digitmode & DSP_DIGITMODE_RELAXDTMF, writeback);
	else
		res = dtmf_detect(&dsp->td.dtmf, s, len, dsp->digitmode & DSP_DIGITMODE_RELAXDTMF, writeback);
	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);
}

static inline int pair_there(float p1, float p2, float i1, float i2, float e)
{
	/* See if p1 and p2 are there, relative to i1 and i2 and total energy */
	/* Make sure absolute levels are high enough */
	if ((p1 < TONE_MIN_THRESH) || (p2 < TONE_MIN_THRESH))
		return 0;
	/* Amplify ignored stuff */
	i2 *= TONE_THRESH;
	i1 *= TONE_THRESH;
	e *= TONE_THRESH;
	/* Check first tone */
	if ((p1 < i1) || (p1 < i2) || (p1 < e))
		return 0;
	/* And second */
	if ((p2 < i1) || (p2 < i2) || (p2 < e))
		return 0;
	/* Guess it's there... */
	return 1;
}

int ast_dsp_getdigits (struct ast_dsp *dsp,
              char *buf,
              int max)
{
	if (dsp->digitmode & DSP_DIGITMODE_MF) {
	    if (max > dsp->td.mf.current_digits)
	        max = dsp->td.mf.current_digits;
	    if (max > 0)
	    {
	        memcpy (buf, dsp->td.mf.digits, max);
	        memmove (dsp->td.mf.digits, dsp->td.mf.digits + max, dsp->td.mf.current_digits - max);
	        dsp->td.mf.current_digits -= max;
	    }
	    buf[max] = '\0';
	    return  max;
	} else {
	    if (max > dsp->td.dtmf.current_digits)
	        max = dsp->td.dtmf.current_digits;
	    if (max > 0)
	    {
	        memcpy (buf, dsp->td.dtmf.digits, max);
	        memmove (dsp->td.dtmf.digits, dsp->td.dtmf.digits + max, dsp->td.dtmf.current_digits - max);
	        dsp->td.dtmf.current_digits -= max;
	    }
	    buf[max] = '\0';
	    return  max;
	}
}

static int __ast_dsp_call_progress(struct ast_dsp *dsp, short *s, int len)
{
	int x;
	int pass;
	int newstate = TONE_STATE_SILENCE;
	int res = 0;
	while(len) {
		/* Take the lesser of the number of samples we need and what we have */
		pass = len;
		if (pass > GSAMP_SIZE - dsp->gsamps) 
			pass = GSAMP_SIZE - dsp->gsamps;
		for (x=0;x<pass;x++) {
			goertzel_sample(&dsp->freqs[HZ_350], s[x]);
			goertzel_sample(&dsp->freqs[HZ_440], s[x]);
			goertzel_sample(&dsp->freqs[HZ_480], s[x]);
			goertzel_sample(&dsp->freqs[HZ_620], s[x]);
			goertzel_sample(&dsp->freqs[HZ_950], s[x]);
			goertzel_sample(&dsp->freqs[HZ_1400], s[x]);
			goertzel_sample(&dsp->freqs[HZ_1800], s[x]);
			dsp->genergy += s[x] * s[x];
		}
		s += pass;
		dsp->gsamps += pass;
		len -= pass;
		if (dsp->gsamps == GSAMP_SIZE) {
			float hz_350;
			float hz_440;
			float hz_480;
			float hz_620;
			float hz_950;
			float hz_1400;
			float hz_1800;
			hz_350 = goertzel_result(&dsp->freqs[HZ_350]);
			hz_440 = goertzel_result(&dsp->freqs[HZ_440]);
			hz_480 = goertzel_result(&dsp->freqs[HZ_480]);
			hz_620 = goertzel_result(&dsp->freqs[HZ_620]);
			hz_950 = goertzel_result(&dsp->freqs[HZ_950]);
			hz_1400 = goertzel_result(&dsp->freqs[HZ_1400]);
			hz_1800 = goertzel_result(&dsp->freqs[HZ_1800]);
#if 0
			printf("Got whole dsp state: 350: %e, 440: %e, 480: %e, 620: %e, 950: %e, 1400: %e, 1800: %e, Energy: %e\n", 
				hz_350, hz_440, hz_480, hz_620, hz_950, hz_1400, hz_1800, dsp->genergy);
#endif
			if (pair_there(hz_480, hz_620, hz_350, hz_440, dsp->genergy)) {
				newstate = TONE_STATE_BUSY;
			} else if (pair_there(hz_440, hz_480, hz_350, hz_620, dsp->genergy)) {
				newstate = TONE_STATE_RINGING;
			} else if (pair_there(hz_350, hz_440, hz_480, hz_620, dsp->genergy)) {
				newstate = TONE_STATE_DIALTONE;
			} else if (hz_950 > TONE_MIN_THRESH * TONE_THRESH) {
				newstate = TONE_STATE_SPECIAL1;
			} else if (hz_1400 > TONE_MIN_THRESH * TONE_THRESH) {
				if (dsp->tstate == TONE_STATE_SPECIAL1)
					newstate = TONE_STATE_SPECIAL2;
			} else if (hz_1800 > TONE_MIN_THRESH * TONE_THRESH) {
				if (dsp->tstate == TONE_STATE_SPECIAL2)
					newstate = TONE_STATE_SPECIAL3;
			} else if (dsp->genergy > TONE_MIN_THRESH * TONE_THRESH) {
				newstate = TONE_STATE_TALKING;
			} else
				newstate = TONE_STATE_SILENCE;
			
			if (newstate == dsp->tstate) {
				dsp->tcount++;
				if (dsp->tcount == COUNT_THRESH) {
					if (dsp->tstate == TONE_STATE_BUSY) {
						res = AST_CONTROL_BUSY;
						dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
					} else if (dsp->tstate == TONE_STATE_TALKING) {
						res = AST_CONTROL_ANSWER;
						dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
					} else if (dsp->tstate == TONE_STATE_RINGING)
						res = AST_CONTROL_RINGING;
					else if (dsp->tstate == TONE_STATE_SPECIAL3) {
						res = AST_CONTROL_CONGESTION;
						dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
					}
					
				}
			} else {
#if 0
				printf("Newstate: %d\n", newstate);
#endif
				dsp->tstate = newstate;
				dsp->tcount = 1;
			}
			
			/* Reset goertzel */						
			for (x=0;x<7;x++)
				dsp->freqs[x].v2 = dsp->freqs[x].v3 = 0.0;
			dsp->gsamps = 0;
			dsp->genergy = 0.0;
		}
	}
#if 0
	if (res)
		printf("Returning %d\n", res);
#endif		
	return res;
}

int ast_dsp_call_progress(struct ast_dsp *dsp, struct ast_frame *inf)
{
	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;
	}
	return __ast_dsp_call_progress(dsp, inf->data, inf->datalen / 2);
}

static int __ast_dsp_silence(struct ast_dsp *dsp, short *s, int len, int *totalsilence)
{
	int accum;
	int x;
	int res = 0;
	
	accum = 0;
	for (x=0;x<len; x++) 
		accum += abs(s[x]);
	accum /= x;
	if (accum < dsp->threshold) {
		dsp->totalsilence += len/8;
		if (dsp->totalnoise) {
			/* Move and save history */
			memmove(dsp->historicnoise, dsp->historicnoise + 1, sizeof(dsp->historicnoise) - sizeof(dsp->historicnoise[0]));
			dsp->historicnoise[DSP_HISTORY - 1] = dsp->totalnoise;
			dsp->busymaybe = 1;
		}
		dsp->totalnoise = 0;
		res = 1;
	} else {
		dsp->totalnoise += len/8;
		if (dsp->totalsilence) {
			/* Move and save history */
			memmove(dsp->historicsilence, dsp->historicsilence + 1, sizeof(dsp->historicsilence) - sizeof(dsp->historicsilence[0]));
			dsp->historicsilence[DSP_HISTORY - 1] = dsp->totalsilence;
			dsp->busymaybe = 1;
		}
		dsp->totalsilence = 0;
	}
	if (totalsilence)
		*totalsilence = dsp->totalsilence;
	return res;
}

int ast_dsp_busydetect(struct ast_dsp *dsp)
{
	int x;
	int res = 0;
	int max, min;
	if (dsp->busymaybe) {
#if 0
		printf("Maybe busy!\n");
#endif		
		dsp->busymaybe = 0;