Main Page | Alphabetical List | Data Structures | File List | Data Fields | Globals

dsp.c

Go to the documentation of this file.
00001 /*
00002  * Asterisk -- A telephony toolkit for Linux.
00003  *
00004  * Convenience Signal Processing routines
00005  * 
00006  * Copyright (C) 2002, Digium
00007  *
00008  * Mark Spencer <markster@linux-support.net>
00009  *
00010  * This program is free software, distributed under the terms of
00011  * the GNU General Public License.
00012  *
00013  * Goertzel routines are borrowed from Steve Underwood's tremendous work on the
00014  * DTMF detector.
00015  *
00016  */
00017 
00018 /* Some routines from tone_detect.c by Steven Underwood as published under the zapata library */
00019 /*
00020    tone_detect.c - General telephony tone detection, and specific
00021                         detection of DTMF.
00022 
00023         Copyright (C) 2001  Steve Underwood <steveu@coppice.org>
00024 
00025         Despite my general liking of the GPL, I place this code in the
00026         public domain for the benefit of all mankind - even the slimy
00027         ones who might try to proprietize my work and use it to my
00028         detriment.
00029 */
00030 
00031 #include <sys/types.h>
00032 #include <asterisk/frame.h>
00033 #include <asterisk/channel.h>
00034 #include <asterisk/channel_pvt.h>
00035 #include <asterisk/logger.h>
00036 #include <asterisk/dsp.h>
00037 #include <asterisk/ulaw.h>
00038 #include <asterisk/alaw.h>
00039 #include <stdlib.h>
00040 #include <unistd.h>
00041 #include <string.h>
00042 #include <math.h>
00043 #include <errno.h>
00044 #include <stdio.h>
00045 
00046 #define DEFAULT_THRESHOLD 1024
00047 
00048 #define BUSY_PERCENT    10 /* The percentage diffrence between the two last silence periods */
00049 #define BUSY_THRESHOLD     100   /* Max number of ms difference between max and min times in busy */
00050 #define BUSY_MIN     75 /* Busy must be at least 80 ms in half-cadence */
00051 #define BUSY_MAX     1100  /* Busy can't be longer than 1100 ms in half-cadence */
00052 
00053 /* Remember last 15 units */
00054 #define DSP_HISTORY 15
00055 
00056 /* Number of goertzels for progress detect */
00057 #define GSAMP_SIZE 183
00058 
00059 #define HZ_350  0
00060 #define HZ_440  1
00061 #define HZ_480  2
00062 #define HZ_620  3
00063 #define HZ_950  4
00064 #define HZ_1400 5
00065 #define HZ_1800 6
00066 
00067 #define TONE_THRESH 10.0   /* How much louder the tone should be than channel energy */
00068 #define TONE_MIN_THRESH 1e8   /* How much tone there should be at least to attempt */
00069 #define COUNT_THRESH  3    /* Need at least 50ms of stuff to count it */
00070 
00071 #define TONE_STATE_SILENCE  0
00072 #define TONE_STATE_RINGING  1 
00073 #define TONE_STATE_DIALTONE 2
00074 #define TONE_STATE_TALKING  3
00075 #define TONE_STATE_BUSY     4
00076 #define TONE_STATE_SPECIAL1   5
00077 #define TONE_STATE_SPECIAL2 6
00078 #define TONE_STATE_SPECIAL3 7
00079 
00080 #define  MAX_DTMF_DIGITS 128
00081 
00082 /* Basic DTMF specs:
00083  *
00084  * Minimum tone on = 40ms
00085  * Minimum tone off = 50ms
00086  * Maximum digit rate = 10 per second
00087  * Normal twist <= 8dB accepted
00088  * Reverse twist <= 4dB accepted
00089  * S/N >= 15dB will detect OK
00090  * Attenuation <= 26dB will detect OK
00091  * Frequency tolerance +- 1.5% will detect, +-3.5% will reject
00092  */
00093 
00094 #define DTMF_THRESHOLD              8.0e7
00095 #define FAX_THRESHOLD              8.0e7
00096 #define FAX_2ND_HARMONIC            2.0     /* 4dB */
00097 #define DTMF_NORMAL_TWIST           6.3     /* 8dB */
00098 #define DTMF_REVERSE_TWIST          ((digitmode & DSP_DIGITMODE_RELAXDTMF) ? 4.0 : 2.5)     /* 4dB normal */
00099 #define DTMF_RELATIVE_PEAK_ROW      6.3     /* 8dB */
00100 #define DTMF_RELATIVE_PEAK_COL      6.3     /* 8dB */
00101 #define DTMF_2ND_HARMONIC_ROW       ((digitmode & DSP_DIGITMODE_RELAXDTMF) ? 1.7 : 2.5)     /* 4dB normal */
00102 #define DTMF_2ND_HARMONIC_COL       63.1    /* 18dB */
00103 #ifndef OLD_DSP_ROUTINES
00104 #define DTMF_TO_TOTAL_ENERGY      42.0
00105 #endif
00106 
00107 #ifdef OLD_DSP_ROUTINES
00108 #define MF_THRESHOLD              8.0e7
00109 #define MF_NORMAL_TWIST           5.3     /* 8dB */
00110 #define MF_REVERSE_TWIST          4.0     /* was 2.5 */
00111 #define MF_RELATIVE_PEAK      5.3     /* 8dB */
00112 #define MF_2ND_HARMONIC       1.7 /* was 2.5  */
00113 #else
00114 #define BELL_MF_THRESHOLD           1.6e9
00115 #define BELL_MF_TWIST               4.0     /* 6dB */
00116 #define BELL_MF_RELATIVE_PEAK       12.6    /* 11dB */
00117 #endif
00118 
00119 typedef struct {
00120    float v2;
00121    float v3;
00122    float fac;
00123 #ifndef OLD_DSP_ROUTINES
00124    int samples;
00125 #endif   
00126 } goertzel_state_t;
00127 
00128 typedef struct
00129 {
00130 
00131     goertzel_state_t row_out[4];
00132     goertzel_state_t col_out[4];
00133    goertzel_state_t fax_tone;
00134 #ifdef OLD_DSP_ROUTINES
00135     goertzel_state_t row_out2nd[4];
00136     goertzel_state_t col_out2nd[4];
00137    goertzel_state_t fax_tone2nd;    
00138     int hit1;
00139     int hit2;
00140     int hit3;
00141     int hit4;
00142 #else
00143     int hits[3];
00144 #endif   
00145     int mhit;
00146     float energy;
00147     int current_sample;
00148 
00149     char digits[MAX_DTMF_DIGITS + 1];
00150     int current_digits;
00151     int detected_digits;
00152     int lost_digits;
00153     int digit_hits[16];
00154 
00155 
00156    int fax_hits;
00157 } dtmf_detect_state_t;
00158 
00159 typedef struct
00160 {
00161     goertzel_state_t tone_out[6];
00162     int mhit;
00163 #ifdef OLD_DSP_ROUTINES
00164     int hit1;
00165     int hit2;
00166     int hit3;
00167     int hit4;
00168     goertzel_state_t tone_out2nd[6];
00169     float energy;
00170 #else
00171     int hits[5];
00172 #endif
00173 
00174     int current_sample;
00175     char digits[MAX_DTMF_DIGITS + 1];
00176     int current_digits;
00177     int detected_digits;
00178     int lost_digits;
00179    int fax_hits;
00180 } mf_detect_state_t;
00181 
00182 static float dtmf_row[] =
00183 {
00184      697.0,  770.0,  852.0,  941.0
00185 };
00186 static float dtmf_col[] =
00187 {
00188     1209.0, 1336.0, 1477.0, 1633.0
00189 };
00190 
00191 static float mf_tones[] =
00192 {
00193    700.0, 900.0, 1100.0, 1300.0, 1500.0, 1700.0
00194 };
00195 
00196 static float fax_freq = 1100.0;
00197 
00198 static char dtmf_positions[] = "123A" "456B" "789C" "*0#D";
00199 
00200 #ifdef OLD_DSP_ROUTINES
00201 static char mf_hit[6][6] = {
00202    /*  700 + */ {   0, '1', '2', '4', '7', 'C' },
00203    /*  900 + */ { '1',   0, '3', '5', '8', 'A' },
00204    /* 1100 + */ { '2', '3',   0, '6', '9', '*' },
00205    /* 1300 + */ { '4', '5', '6',   0, '0', 'B' },
00206    /* 1500 + */ { '7', '8', '9', '0',  0, '#' },
00207    /* 1700 + */ { 'C', 'A', '*', 'B', '#',  0  },
00208 };
00209 #else
00210 static char bell_mf_positions[] = "1247C-358A--69*---0B----#";
00211 #endif
00212 
00213 static inline void goertzel_sample(goertzel_state_t *s, short sample)
00214 {
00215    float v1;
00216    float fsamp  = sample;
00217    v1 = s->v2;
00218    s->v2 = s->v3;
00219    s->v3 = s->fac * s->v2 - v1 + fsamp;
00220 }
00221 
00222 static inline void goertzel_update(goertzel_state_t *s, short *samps, int count)
00223 {
00224    int i;
00225    for (i=0;i<count;i++) 
00226       goertzel_sample(s, samps[i]);
00227 }
00228 
00229 
00230 static inline float goertzel_result(goertzel_state_t *s)
00231 {
00232    return s->v3 * s->v3 + s->v2 * s->v2 - s->v2 * s->v3 * s->fac;
00233 }
00234 
00235 static inline void goertzel_init(goertzel_state_t *s, float freq, int samples)
00236 {
00237    s->v2 = s->v3 = 0.0;
00238    s->fac = 2.0 * cos(2.0 * M_PI * (freq / 8000.0));
00239 #ifndef OLD_DSP_ROUTINES
00240    s->samples = samples;
00241 #endif
00242 }
00243 
00244 static inline void goertzel_reset(goertzel_state_t *s)
00245 {
00246    s->v2 = s->v3 = 0.0;
00247 }
00248 
00249 struct ast_dsp {
00250    struct ast_frame f;
00251    int threshold;
00252    int totalsilence;
00253    int totalnoise;
00254    int features;
00255    int busymaybe;
00256    int busycount;
00257    int historicnoise[DSP_HISTORY];
00258    int historicsilence[DSP_HISTORY];
00259    goertzel_state_t freqs[7];
00260    int gsamps;
00261    int tstate;
00262    int tcount;
00263    int digitmode;
00264    int thinkdigit;
00265    float genergy;
00266    union {
00267       dtmf_detect_state_t dtmf;
00268       mf_detect_state_t mf;
00269    } td;
00270 };
00271 
00272 static void ast_dtmf_detect_init (dtmf_detect_state_t *s)
00273 {
00274     int i;
00275 
00276 #ifdef OLD_DSP_ROUTINES
00277     s->hit1 = 
00278     s->mhit = 
00279    s->hit3 =
00280    s->hit4 = 
00281     s->hit2 = 0;
00282 #else
00283    s->hits[0] = s->hits[1] = s->hits[2] = 0;
00284 #endif
00285     for (i = 0;  i < 4;  i++)
00286     {
00287     
00288          goertzel_init (&s->row_out[i], dtmf_row[i], 102);
00289       goertzel_init (&s->col_out[i], dtmf_col[i], 102);
00290 #ifdef OLD_DSP_ROUTINES
00291       goertzel_init (&s->row_out2nd[i], dtmf_row[i] * 2.0, 102);
00292       goertzel_init (&s->col_out2nd[i], dtmf_col[i] * 2.0, 102);
00293 #endif   
00294       s->energy = 0.0;
00295     }
00296 
00297    /* Same for the fax dector */
00298     goertzel_init (&s->fax_tone, fax_freq, 102);
00299 
00300 #ifdef OLD_DSP_ROUTINES
00301    /* Same for the fax dector 2nd harmonic */
00302     goertzel_init (&s->fax_tone2nd, fax_freq * 2.0, 102);
00303 #endif   
00304    
00305     s->current_sample = 0;
00306     s->detected_digits = 0;
00307    s->current_digits = 0;
00308    memset(&s->digits, 0, sizeof(s->digits));
00309     s->lost_digits = 0;
00310     s->digits[0] = '\0';
00311 }
00312 
00313 static void ast_mf_detect_init (mf_detect_state_t *s)
00314 {
00315     int i;
00316 
00317 #ifdef OLD_DSP_ROUTINES
00318     s->hit1 = 
00319     s->hit2 = 0;
00320 #else 
00321    s->hits[0] = s->hits[1] = s->hits[2] = s->hits[3] = s->hits[4] = 0;
00322 #endif
00323     for (i = 0;  i < 6;  i++)
00324     {
00325     
00326          goertzel_init (&s->tone_out[i], mf_tones[i], 160);
00327 #ifdef OLD_DSP_ROUTINES
00328       goertzel_init (&s->tone_out2nd[i], mf_tones[i] * 2.0, 160);
00329       s->energy = 0.0;
00330 #endif
00331    
00332     }
00333 
00334    s->current_digits = 0;
00335    memset(&s->digits, 0, sizeof(s->digits));
00336     s->current_sample = 0;
00337     s->detected_digits = 0;
00338     s->lost_digits = 0;
00339     s->digits[0] = '\0';
00340     s->mhit = 0;
00341 }
00342 
00343 static int dtmf_detect (dtmf_detect_state_t *s,
00344                  int16_t amp[],
00345                  int samples, 
00346        int digitmode, int *writeback)
00347 {
00348 
00349     float row_energy[4];
00350     float col_energy[4];
00351     float fax_energy;
00352 #ifdef OLD_DSP_ROUTINES
00353     float fax_energy_2nd;
00354 #endif   
00355     float famp;
00356     float v1;
00357     int i;
00358     int j;
00359     int sample;
00360     int best_row;
00361     int best_col;
00362     int hit;
00363     int limit;
00364 
00365     hit = 0;
00366     for (sample = 0;  sample < samples;  sample = limit)
00367     {
00368         /* 102 is optimised to meet the DTMF specs. */
00369         if ((samples - sample) >= (102 - s->current_sample))
00370             limit = sample + (102 - s->current_sample);
00371         else
00372             limit = samples;
00373 #if defined(USE_3DNOW)
00374         _dtmf_goertzel_update (s->row_out, amp + sample, limit - sample);
00375         _dtmf_goertzel_update (s->col_out, amp + sample, limit - sample);
00376 #ifdef OLD_DSP_ROUTINES
00377         _dtmf_goertzel_update (s->row_out2nd, amp + sample, limit2 - sample);
00378         _dtmf_goertzel_update (s->col_out2nd, amp + sample, limit2 - sample);
00379 #endif      
00380       /* XXX Need to fax detect for 3dnow too XXX */
00381       #warning "Fax Support Broken"
00382 #else
00383         /* The following unrolled loop takes only 35% (rough estimate) of the 
00384            time of a rolled loop on the machine on which it was developed */
00385         for (j = sample;  j < limit;  j++)
00386         {
00387             famp = amp[j];
00388        
00389        s->energy += famp*famp;
00390        
00391             /* With GCC 2.95, the following unrolled code seems to take about 35%
00392                (rough estimate) as long as a neat little 0-3 loop */
00393             v1 = s->row_out[0].v2;
00394             s->row_out[0].v2 = s->row_out[0].v3;
00395             s->row_out[0].v3 = s->row_out[0].fac*s->row_out[0].v2 - v1 + famp;
00396     
00397             v1 = s->col_out[0].v2;
00398             s->col_out[0].v2 = s->col_out[0].v3;
00399             s->col_out[0].v3 = s->col_out[0].fac*s->col_out[0].v2 - v1 + famp;
00400     
00401             v1 = s->row_out[1].v2;
00402             s->row_out[1].v2 = s->row_out[1].v3;
00403             s->row_out[1].v3 = s->row_out[1].fac*s->row_out[1].v2 - v1 + famp;
00404     
00405             v1 = s->col_out[1].v2;
00406             s->col_out[1].v2 = s->col_out[1].v3;
00407             s->col_out[1].v3 = s->col_out[1].fac*s->col_out[1].v2 - v1 + famp;
00408     
00409             v1 = s->row_out[2].v2;
00410             s->row_out[2].v2 = s->row_out[2].v3;
00411             s->row_out[2].v3 = s->row_out[2].fac*s->row_out[2].v2 - v1 + famp;
00412     
00413             v1 = s->col_out[2].v2;
00414             s->col_out[2].v2 = s->col_out[2].v3;
00415             s->col_out[2].v3 = s->col_out[2].fac*s->col_out[2].v2 - v1 + famp;
00416     
00417             v1 = s->row_out[3].v2;
00418             s->row_out[3].v2 = s->row_out[3].v3;
00419             s->row_out[3].v3 = s->row_out[3].fac*s->row_out[3].v2 - v1 + famp;
00420 
00421             v1 = s->col_out[3].v2;
00422             s->col_out[3].v2 = s->col_out[3].v3;
00423             s->col_out[3].v3 = s->col_out[3].fac*s->col_out[3].v2 - v1 + famp;
00424 
00425          /* Update fax tone */
00426             v1 = s->fax_tone.v2;
00427             s->fax_tone.v2 = s->fax_tone.v3;
00428             s->fax_tone.v3 = s->fax_tone.fac*s->fax_tone.v2 - v1 + famp;
00429 
00430 #ifdef OLD_DSP_ROUTINES
00431             v1 = s->col_out2nd[0].v2;
00432             s->col_out2nd[0].v2 = s->col_out2nd[0].v3;
00433             s->col_out2nd[0].v3 = s->col_out2nd[0].fac*s->col_out2nd[0].v2 - v1 + famp;
00434         
00435             v1 = s->row_out2nd[0].v2;
00436             s->row_out2nd[0].v2 = s->row_out2nd[0].v3;
00437             s->row_out2nd[0].v3 = s->row_out2nd[0].fac*s->row_out2nd[0].v2 - v1 + famp;
00438         
00439             v1 = s->col_out2nd[1].v2;
00440             s->col_out2nd[1].v2 = s->col_out2nd[1].v3;
00441             s->col_out2nd[1].v3 = s->col_out2nd[1].fac*s->col_out2nd[1].v2 - v1 + famp;
00442     
00443             v1 = s->row_out2nd[1].v2;
00444             s->row_out2nd[1].v2 = s->row_out2nd[1].v3;
00445             s->row_out2nd[1].v3 = s->row_out2nd[1].fac*s->row_out2nd[1].v2 - v1 + famp;
00446         
00447             v1 = s->col_out2nd[2].v2;
00448             s->col_out2nd[2].v2 = s->col_out2nd[2].v3;
00449             s->col_out2nd[2].v3 = s->col_out2nd[2].fac*s->col_out2nd[2].v2 - v1 + famp;
00450         
00451             v1 = s->row_out2nd[2].v2;
00452             s->row_out2nd[2].v2 = s->row_out2nd[2].v3;
00453             s->row_out2nd[2].v3 = s->row_out2nd[2].fac*s->row_out2nd[2].v2 - v1 + famp;
00454         
00455             v1 = s->col_out2nd[3].v2;
00456             s->col_out2nd[3].v2 = s->col_out2nd[3].v3;
00457             s->col_out2nd[3].v3 = s->col_out2nd[3].fac*s->col_out2nd[3].v2 - v1 + famp;
00458         
00459             v1 = s->row_out2nd[3].v2;
00460             s->row_out2nd[3].v2 = s->row_out2nd[3].v3;
00461             s->row_out2nd[3].v3 = s->row_out2nd[3].fac*s->row_out2nd[3].v2 - v1 + famp;
00462 
00463             v1 = s->fax_tone.v2;
00464             s->fax_tone2nd.v2 = s->fax_tone2nd.v3;
00465             s->fax_tone2nd.v3 = s->fax_tone2nd.fac*s->fax_tone2nd.v2 - v1 + famp;
00466 #endif
00467         }
00468 #endif
00469         s->current_sample += (limit - sample);
00470         if (s->current_sample < 102) {
00471          if (hit && !((digitmode & DSP_DIGITMODE_NOQUELCH))) {
00472             /* If we had a hit last time, go ahead and clear this out since likely it
00473                will be another hit */
00474             for (i=sample;i<limit;i++) 
00475                amp[i] = 0;
00476             *writeback = 1;
00477          }
00478             continue;
00479       }
00480 
00481       /* Detect the fax energy, too */
00482       fax_energy = goertzel_result(&s->fax_tone);
00483       
00484         /* We are at the end of a DTMF detection block */
00485         /* Find the peak row and the peak column */
00486         row_energy[0] = goertzel_result (&s->row_out[0]);
00487         col_energy[0] = goertzel_result (&s->col_out[0]);
00488 
00489    for (best_row = best_col = 0, i = 1;  i < 4;  i++)
00490    {
00491           row_energy[i] = goertzel_result (&s->row_out[i]);
00492             if (row_energy[i] > row_energy[best_row])
00493                 best_row = i;
00494           col_energy[i] = goertzel_result (&s->col_out[i]);
00495             if (col_energy[i] > col_energy[best_col])
00496                 best_col = i;
00497       }
00498         hit = 0;
00499         /* Basic signal level test and the twist test */
00500         if (row_energy[best_row] >= DTMF_THRESHOLD
00501        &&
00502        col_energy[best_col] >= DTMF_THRESHOLD
00503             &&
00504             col_energy[best_col] < row_energy[best_row]*DTMF_REVERSE_TWIST
00505             &&
00506             col_energy[best_col]*DTMF_NORMAL_TWIST > row_energy[best_row])
00507         {
00508             /* Relative peak test */
00509             for (i = 0;  i < 4;  i++)
00510             {
00511                 if ((i != best_col  &&  col_energy[i]*DTMF_RELATIVE_PEAK_COL > col_energy[best_col])
00512                     ||
00513                     (i != best_row  &&  row_energy[i]*DTMF_RELATIVE_PEAK_ROW > row_energy[best_row]))
00514                 {
00515                     break;
00516                 }
00517             }
00518 #ifdef OLD_DSP_ROUTINES
00519             /* ... and second harmonic test */
00520             if (i >= 4
00521            &&
00522       (row_energy[best_row] + col_energy[best_col]) > 42.0*s->energy
00523                 &&
00524                 goertzel_result (&s->col_out2nd[best_col])*DTMF_2ND_HARMONIC_COL < col_energy[best_col]
00525                 &&
00526                 goertzel_result (&s->row_out2nd[best_row])*DTMF_2ND_HARMONIC_ROW < row_energy[best_row])
00527 #else
00528             /* ... and fraction of total energy test */
00529             if (i >= 4
00530                 &&
00531                 (row_energy[best_row] + col_energy[best_col]) > DTMF_TO_TOTAL_ENERGY*s->energy)
00532 #endif
00533             {
00534             /* Got a hit */
00535                 hit = dtmf_positions[(best_row << 2) + best_col];
00536             if (!(digitmode & DSP_DIGITMODE_NOQUELCH)) {
00537                /* Zero out frame data if this is part DTMF */
00538                for (i=sample;i<limit;i++) 
00539                   amp[i] = 0;
00540                *writeback = 1;
00541             }
00542                 /* Look for two successive similar results */
00543                 /* The logic in the next test is:
00544                    We need two successive identical clean detects, with
00545          something different preceeding it. This can work with
00546          back to back differing digits. More importantly, it
00547          can work with nasty phones that give a very wobbly start
00548          to a digit. */
00549          
00550 #ifdef OLD_DSP_ROUTINES
00551                 if (hit == s->hit3  &&  s->hit3 != s->hit2)
00552                 {
00553                 s->mhit = hit;
00554                     s->digit_hits[(best_row << 2) + best_col]++;
00555                     s->detected_digits++;
00556                     if (s->current_digits < MAX_DTMF_DIGITS)
00557                     {
00558                         s->digits[s->current_digits++] = hit;
00559                         s->digits[s->current_digits] = '\0';
00560                     }
00561                     else
00562                     {
00563                         s->lost_digits++;
00564                     }
00565                 }
00566 #else          
00567                 if (hit == s->hits[2]  &&  hit != s->hits[1]  &&  hit != s->hits[0])
00568                 {
00569                 s->mhit = hit;
00570                     s->digit_hits[(best_row << 2) + best_col]++;
00571                     s->detected_digits++;
00572                     if (s->current_digits < MAX_DTMF_DIGITS)
00573                     {
00574                         s->digits[s->current_digits++] = hit;
00575                         s->digits[s->current_digits] = '\0';
00576                     }
00577                     else
00578                     {
00579                         s->lost_digits++;
00580                     }
00581                 }
00582 #endif
00583             }
00584         } 
00585 #ifdef OLD_DSP_ROUTINES
00586       if (!hit && (fax_energy >= FAX_THRESHOLD) && (fax_energy > s->energy * 21.0)) {
00587             fax_energy_2nd = goertzel_result(&s->fax_tone2nd);
00588             fax_energy_2nd = goertzel_result(&s->fax_tone2nd);
00589             if (fax_energy_2nd * FAX_2ND_HARMONIC < fax_energy) {
00590 #if 0
00591                printf("Fax energy/Second Harmonic: %f/%f\n", fax_energy, fax_energy_2nd);
00592 #endif               
00593                /* XXX Probably need better checking than just this the energy XXX */
00594                hit = 'f';
00595                s->fax_hits++;
00596             } /* Don't reset fax hits counter */
00597       }
00598 #else /* OLD_DSP_ROUTINES */
00599       if (!hit && (fax_energy >= FAX_THRESHOLD) && (fax_energy >= DTMF_TO_TOTAL_ENERGY*s->energy)) {
00600 #if 0
00601             printf("Fax energy/Second Harmonic: %f\n", fax_energy);
00602 #endif               
00603                /* XXX Probably need better checking than just this the energy XXX */
00604             hit = 'f';
00605             s->fax_hits++;
00606       }
00607 #endif /* OLD_DSP_ROUTINES */
00608       else {
00609          if (s->fax_hits > 5) {
00610              hit = 'f';
00611              s->mhit = 'f';
00612                 s->detected_digits++;
00613                 if (s->current_digits < MAX_DTMF_DIGITS)
00614                 {
00615                      s->digits[s->current_digits++] = hit;
00616                      s->digits[s->current_digits] = '\0';
00617                 }
00618                 else
00619                 {
00620                       s->lost_digits++;
00621                 }
00622          }
00623          s->fax_hits = 0;
00624       }
00625 #ifdef OLD_DSP_ROUTINES
00626         s->hit1 = s->hit2;
00627         s->hit2 = s->hit3;
00628         s->hit3 = hit;
00629 #else
00630         s->hits[0] = s->hits[1];
00631         s->hits[1] = s->hits[2];
00632         s->hits[2] = hit;
00633 #endif      
00634         /* Reinitialise the detector for the next block */
00635         for (i = 0;  i < 4;  i++)
00636         {
00637              goertzel_reset(&s->row_out[i]);
00638             goertzel_reset(&s->col_out[i]);
00639 #ifdef OLD_DSP_ROUTINES
00640           goertzel_reset(&s->row_out2nd[i]);
00641           goertzel_reset(&s->col_out2nd[i]);
00642 #endif         
00643         }
00644       goertzel_reset (&s->fax_tone);
00645 #ifdef OLD_DSP_ROUTINES
00646       goertzel_reset (&s->fax_tone2nd);
00647 #endif         
00648       s->energy = 0.0;
00649         s->current_sample = 0;
00650     }
00651     if ((!s->mhit) || (s->mhit != hit))
00652     {
00653    s->mhit = 0;
00654    return(0);
00655     }
00656     return (hit);
00657 }
00658 
00659 /* MF goertzel size */
00660 #ifdef OLD_DSP_ROUTINES
00661 #define  MF_GSIZE 160
00662 #else
00663 #define MF_GSIZE 120
00664 #endif
00665 
00666 static int mf_detect (mf_detect_state_t *s,
00667                  int16_t amp[],
00668                  int samples, 
00669        int digitmode, int *writeback)
00670 {
00671 
00672 #ifdef OLD_DSP_ROUTINES
00673     float tone_energy[6];
00674     int best1;
00675     int best2;
00676    float max;
00677    int sofarsogood;
00678 #else
00679     float energy[6];
00680     int best;
00681     int second_best;
00682 #endif
00683     float famp;
00684     float v1;
00685     int i;
00686     int j;
00687     int sample;
00688     int hit;
00689     int limit;
00690 
00691     hit = 0;
00692     for (sample = 0;  sample < samples;  sample = limit)
00693     {
00694         /* 80 is optimised to meet the MF specs. */
00695         if ((samples - sample) >= (MF_GSIZE - s->current_sample))
00696             limit = sample + (MF_GSIZE - s->current_sample);
00697         else
00698             limit = samples;
00699 #if defined(USE_3DNOW)
00700         _dtmf_goertzel_update (s->row_out, amp + sample, limit - sample);
00701         _dtmf_goertzel_update (s->col_out, amp + sample, limit - sample);
00702 #ifdef OLD_DSP_ROUTINES
00703         _dtmf_goertzel_update (s->row_out2nd, amp + sample, limit2 - sample);
00704         _dtmf_goertzel_update (s->col_out2nd, amp + sample, limit2 - sample);
00705 #endif
00706       /* XXX Need to fax detect for 3dnow too XXX */
00707       #warning "Fax Support Broken"
00708 #else
00709         /* The following unrolled loop takes only 35% (rough estimate) of the 
00710            time of a rolled loop on the machine on which it was developed */
00711         for (j = sample;  j < limit;  j++)
00712         {
00713             famp = amp[j];
00714        
00715 #ifdef OLD_DSP_ROUTINES
00716        s->energy += famp*famp;
00717 #endif
00718        
00719             /* With GCC 2.95, the following unrolled code seems to take about 35%
00720                (rough estimate) as long as a neat little 0-3 loop */
00721             v1 = s->tone_out[0].v2;
00722             s->tone_out[0].v2 = s->tone_out[0].v3;
00723             s->tone_out[0].v3 = s->tone_out[0].fac*s->tone_out[0].v2 - v1 + famp;
00724 
00725             v1 = s->tone_out[1].v2;
00726             s->tone_out[1].v2 = s->tone_out[1].v3;
00727             s->tone_out[1].v3 = s->tone_out[1].fac*s->tone_out[1].v2 - v1 + famp;
00728     
00729             v1 = s->tone_out[2].v2;
00730             s->tone_out[2].v2 = s->tone_out[2].v3;
00731             s->tone_out[2].v3 = s->tone_out[2].fac*s->tone_out[2].v2 - v1 + famp;
00732     
00733             v1 = s->tone_out[3].v2;
00734             s->tone_out[3].v2 = s->tone_out[3].v3;
00735             s->tone_out[3].v3 = s->tone_out[3].fac*s->tone_out[3].v2 - v1 + famp;
00736 
00737             v1 = s->tone_out[4].v2;
00738             s->tone_out[4].v2 = s->tone_out[4].v3;
00739             s->tone_out[4].v3 = s->tone_out[4].fac*s->tone_out[4].v2 - v1 + famp;
00740 
00741             v1 = s->tone_out[5].v2;
00742             s->tone_out[5].v2 = s->tone_out[5].v3;
00743             s->tone_out[5].v3 = s->tone_out[5].fac*s->tone_out[5].v2 - v1 + famp;
00744 
00745 #ifdef OLD_DSP_ROUTINES
00746             v1 = s->tone_out2nd[0].v2;
00747             s->tone_out2nd[0].v2 = s->tone_out2nd[0].v3;
00748             s->tone_out2nd[0].v3 = s->tone_out2nd[0].fac*s->tone_out2nd[0].v2 - v1 + famp;
00749         
00750             v1 = s->tone_out2nd[1].v2;
00751             s->tone_out2nd[1].v2 = s->tone_out2nd[1].v3;
00752             s->tone_out2nd[1].v3 = s->tone_out2nd[1].fac*s->tone_out2nd[1].v2 - v1 + famp;
00753         
00754             v1 = s->tone_out2nd[2].v2;
00755             s->tone_out2nd[2].v2 = s->tone_out2nd[2].v3;
00756             s->tone_out2nd[2].v3 = s->tone_out2nd[2].fac*s->tone_out2nd[2].v2 - v1 + famp;
00757         
00758             v1 = s->tone_out2nd[3].v2;
00759             s->tone_out2nd[3].v2 = s->tone_out2nd[3].v3;
00760             s->tone_out2nd[3].v3 = s->tone_out2nd[3].fac*s->tone_out2nd[3].v2 - v1 + famp;
00761 
00762             v1 = s->tone_out2nd[4].v2;
00763             s->tone_out2nd[4].v2 = s->tone_out2nd[4].v3;
00764             s->tone_out2nd[4].v3 = s->tone_out2nd[4].fac*s->tone_out2nd[2].v2 - v1 + famp;
00765         
00766             v1 = s->tone_out2nd[3].v2;
00767             s->tone_out2nd[5].v2 = s->tone_out2nd[6].v3;
00768             s->tone_out2nd[5].v3 = s->tone_out2nd[6].fac*s->tone_out2nd[3].v2 - v1 + famp;
00769 #endif
00770         }
00771 #endif
00772         s->current_sample += (limit - sample);
00773         if (s->current_sample < MF_GSIZE) {
00774          if (hit && !((digitmode & DSP_DIGITMODE_NOQUELCH))) {
00775             /* If we had a hit last time, go ahead and clear this out since likely it
00776                will be another hit */
00777             for (i=sample;i<limit;i++) 
00778                amp[i] = 0;
00779             *writeback = 1;
00780          }
00781             continue;
00782       }
00783 
00784 
00785 #ifdef OLD_DSP_ROUTINES    
00786       /* We're at the end of an MF detection block.  Go ahead and calculate
00787          all the energies. */
00788       for (i=0;i<6;i++) {
00789          tone_energy[i] = goertzel_result(&s->tone_out[i]);
00790       }
00791       /* Find highest */
00792       best1 = 0;
00793       max = tone_energy[0];
00794       for (i=1;i<6;i++) {
00795          if (tone_energy[i] > max) {
00796             max = tone_energy[i];
00797             best1 = i;
00798          }
00799       }
00800 
00801       /* Find 2nd highest */
00802       if (best1) {
00803          max = tone_energy[0];
00804          best2 = 0;
00805       } else {
00806          max = tone_energy[1];
00807          best2 = 1;
00808       }
00809 
00810       for (i=0;i<6;i++) {
00811          if (i == best1) continue;
00812          if (tone_energy[i] > max) {
00813             max = tone_energy[i];
00814             best2 = i;
00815          }
00816       }
00817             
00818         hit = 0;
00819       if (best1 != best2) sofarsogood=1;
00820       else sofarsogood=0;
00821       /* Check for relative energies */
00822       for (i=0;i<6;i++) {
00823          if (i == best1) continue;
00824          if (i == best2) continue;
00825          if (tone_energy[best1] < tone_energy[i] * MF_RELATIVE_PEAK) {
00826             sofarsogood = 0;
00827             break;
00828          }
00829          if (tone_energy[best2] < tone_energy[i] * MF_RELATIVE_PEAK) {
00830             sofarsogood = 0;
00831             break;
00832          }
00833       }
00834       
00835       if (sofarsogood) {
00836          /* Check for 2nd harmonic */
00837          if (goertzel_result(&s->tone_out2nd[best1]) * MF_2ND_HARMONIC > tone_energy[best1]) 
00838             sofarsogood = 0;
00839          else if (goertzel_result(&s->tone_out2nd[best2]) * MF_2ND_HARMONIC > tone_energy[best2])
00840             sofarsogood = 0;
00841       }
00842       if (sofarsogood) {
00843          hit = mf_hit[best1][best2];
00844          if (!(digitmode & DSP_DIGITMODE_NOQUELCH)) {
00845             /* Zero out frame data if this is part DTMF */
00846             for (i=sample;i<limit;i++) 
00847                amp[i] = 0;
00848             *writeback = 1;
00849          }
00850          /* Look for two consecutive clean hits */
00851          if ((hit == s->hit3) && (s->hit3 != s->hit2)) {
00852             s->mhit = hit;
00853             s->detected_digits++;
00854             if (s->current_digits < MAX_DTMF_DIGITS - 2) {
00855                s->digits[s->current_digits++] = hit;
00856                s->digits[s->current_digits] = '\0';
00857             } else {
00858                s->lost_digits++;
00859             }
00860          }
00861       }
00862       
00863         s->hit1 = s->hit2;
00864         s->hit2 = s->hit3;
00865         s->hit3 = hit;
00866         /* Reinitialise the detector for the next block */
00867         for (i = 0;  i < 6;  i++)
00868         {
00869              goertzel_reset(&s->tone_out[i]);
00870             goertzel_reset(&s->tone_out2nd[i]);
00871         }
00872       s->energy = 0.0;
00873         s->current_sample = 0;
00874     }
00875 #else
00876       /* We're at the end of an MF detection block.  */
00877         /* Find the two highest energies. The spec says to look for
00878            two tones and two tones only. Taking this literally -ie
00879            only two tones pass the minimum threshold - doesn't work
00880            well. The sinc function mess, due to rectangular windowing
00881            ensure that! Find the two highest energies and ensure they
00882            are considerably stronger than any of the others. */
00883         energy[0] = goertzel_result(&s->tone_out[0]);
00884         energy[1] = goertzel_result(&s->tone_out[1]);
00885         if (energy[0] > energy[1])
00886         {
00887             best = 0;
00888             second_best = 1;
00889         }
00890         else
00891         {
00892             best = 1;
00893             second_best = 0;
00894         }
00895         /*endif*/
00896         for (i = 2;  i < 6;  i++)
00897         {
00898             energy[i] = goertzel_result(&s->tone_out[i]);
00899             if (energy[i] >= energy[best])
00900             {
00901                 second_best = best;
00902                 best = i;
00903             }
00904             else if (energy[i] >= energy[second_best])
00905             {
00906                 second_best = i;
00907             }
00908         }
00909         /* Basic signal level and twist tests */
00910         hit = 0;
00911         if (energy[best] >= BELL_MF_THRESHOLD
00912             &&
00913             energy[second_best] >= BELL_MF_THRESHOLD
00914             &&
00915             energy[best] < energy[second_best]*BELL_MF_TWIST
00916             &&
00917             energy[best]*BELL_MF_TWIST > energy[second_best])
00918         {
00919             /* Relative peak test */
00920             hit = -1;
00921             for (i = 0;  i < 6;  i++)
00922             {
00923                 if (i != best  &&  i != second_best)
00924                 {
00925                     if (energy[i]*BELL_MF_RELATIVE_PEAK >= energy[second_best])
00926                     {
00927                         /* The best two are not clearly the best */
00928                         hit = 0;
00929                         break;
00930                     }
00931                 }
00932             }
00933         }
00934         if (hit)
00935         {
00936             /* Get the values into ascending order */
00937             if (second_best < best)
00938             {
00939                 i = best;
00940                 best = second_best;
00941                 second_best = i;
00942             }
00943             best = best*5 + second_best - 1;
00944             hit = bell_mf_positions[best];
00945             /* Look for two successive similar results */
00946             /* The logic in the next test is:
00947                For KP we need 4 successive identical clean detects, with
00948                two blocks of something different preceeding it. For anything
00949                else we need two successive identical clean detects, with
00950                two blocks of something different preceeding it. */
00951             if (hit == s->hits[4]
00952                 &&
00953                 hit == s->hits[3]
00954                 &&
00955                    ((hit != '*'  &&  hit != s->hits[2]  &&  hit != s->hits[1])
00956                     ||
00957                     (hit == '*'  &&  hit == s->hits[2]  &&  hit != s->hits[1]  &&  hit != s->hits[0])))
00958             {
00959                 s->detected_digits++;
00960                 if (s->current_digits < MAX_DTMF_DIGITS)
00961                 {
00962                     s->digits[s->current_digits++] = hit;
00963                     s->digits[s->current_digits] = '\0';
00964                 }
00965                 else
00966                 {
00967                     s->lost_digits++;
00968                 }
00969             }
00970         }
00971         else
00972         {
00973             hit = 0;
00974         }
00975         s->hits[0] = s->hits[1];
00976         s->hits[1] = s->hits[2];
00977         s->hits[2] = s->hits[3];
00978         s->hits[3] = s->hits[4];
00979         s->hits[4] = hit;
00980         /* Reinitialise the detector for the next block */
00981         for (i = 0;  i < 6;  i++)
00982              goertzel_reset(&s->tone_out[i]);
00983         s->current_sample = 0;
00984     }
00985 #endif   
00986     if ((!s->mhit) || (s->mhit != hit))
00987     {
00988       s->mhit = 0;
00989       return(0);
00990     }
00991     return (hit);
00992 }
00993 
00994 static int __ast_dsp_digitdetect(struct ast_dsp *dsp, short *s, int len, int *writeback)
00995 {
00996    int res;
00997    if (dsp->digitmode & DSP_DIGITMODE_MF)
00998       res = mf_detect(&dsp->td.mf, s, len, dsp->digitmode & DSP_DIGITMODE_RELAXDTMF, writeback);
00999    else
01000       res = dtmf_detect(&dsp->td.dtmf, s, len, dsp->digitmode & DSP_DIGITMODE_RELAXDTMF, writeback);
01001    return res;
01002 }
01003 
01004 int ast_dsp_digitdetect(struct ast_dsp *dsp, struct ast_frame *inf)
01005 {
01006    short *s;
01007    int len;
01008    int ign=0;
01009    if (inf->frametype != AST_FRAME_VOICE) {
01010       ast_log(LOG_WARNING, "Can't check call progress of non-voice frames\n");
01011       return 0;
01012    }
01013    if (inf->subclass != AST_FORMAT_SLINEAR) {
01014       ast_log(LOG_WARNING, "Can only check call progress in signed-linear frames\n");
01015       return 0;
01016    }
01017    s = inf->data;
01018    len = inf->datalen / 2;
01019    return __ast_dsp_digitdetect(dsp, s, len, &ign);
01020 }
01021 
01022 static inline int pair_there(float p1, float p2, float i1, float i2, float e)
01023 {
01024    /* See if p1 and p2 are there, relative to i1 and i2 and total energy */
01025    /* Make sure absolute levels are high enough */
01026    if ((p1 < TONE_MIN_THRESH) || (p2 < TONE_MIN_THRESH))
01027       return 0;
01028    /* Amplify ignored stuff */
01029    i2 *= TONE_THRESH;
01030    i1 *= TONE_THRESH;
01031    e *= TONE_THRESH;
01032    /* Check first tone */
01033    if ((p1 < i1) || (p1 < i2) || (p1 < e))
01034       return 0;
01035    /* And second */
01036    if ((p2 < i1) || (p2 < i2) || (p2 < e))
01037       return 0;
01038    /* Guess it's there... */
01039    return 1;
01040 }
01041 
01042 int ast_dsp_getdigits (struct ast_dsp *dsp,
01043               char *buf,
01044               int max)
01045 {
01046    if (dsp->digitmode & DSP_DIGITMODE_MF) {
01047        if (max > dsp->td.mf.current_digits)
01048            max = dsp->td.mf.current_digits;
01049        if (max > 0)
01050        {
01051            memcpy (buf, dsp->td.mf.digits, max);
01052            memmove (dsp->td.mf.digits, dsp->td.mf.digits + max, dsp->td.mf.current_digits - max);
01053            dsp->td.mf.current_digits -= max;
01054        }
01055        buf[max] = '\0';
01056        return  max;
01057    } else {
01058        if (max > dsp->td.dtmf.current_digits)
01059            max = dsp->td.dtmf.current_digits;
01060        if (max > 0)
01061        {
01062            memcpy (buf, dsp->td.dtmf.digits, max);
01063            memmove (dsp->td.dtmf.digits, dsp->td.dtmf.digits + max, dsp->td.dtmf.current_digits - max);
01064            dsp->td.dtmf.current_digits -= max;
01065        }
01066        buf[max] = '\0';
01067        return  max;
01068    }
01069 }
01070 
01071 static int __ast_dsp_call_progress(struct ast_dsp *dsp, short *s, int len)
01072 {
01073    int x;
01074    int pass;
01075    int newstate = TONE_STATE_SILENCE;
01076    int res = 0;
01077    while(len) {
01078       /* Take the lesser of the number of samples we need and what we have */
01079       pass = len;
01080       if (pass > GSAMP_SIZE - dsp->gsamps) 
01081          pass = GSAMP_SIZE - dsp->gsamps;
01082       for (x=0;x<pass;x++) {
01083          goertzel_sample(&dsp->freqs[HZ_350], s[x]);
01084          goertzel_sample(&dsp->freqs[HZ_440], s[x]);
01085          goertzel_sample(&dsp->freqs[HZ_480], s[x]);
01086          goertzel_sample(&dsp->freqs[HZ_620], s[x]);
01087          goertzel_sample(&dsp->freqs[HZ_950], s[x]);
01088          goertzel_sample(&dsp->freqs[HZ_1400], s[x]);
01089          goertzel_sample(&dsp->freqs[HZ_1800], s[x]);
01090          dsp->genergy += s[x] * s[x];
01091       }
01092       s += pass;
01093       dsp->gsamps += pass;
01094       len -= pass;
01095       if (dsp->gsamps == GSAMP_SIZE) {
01096          float hz_350;
01097          float hz_440;
01098          float hz_480;
01099          float hz_620;
01100          float hz_950;
01101          float hz_1400;
01102          float hz_1800;
01103          hz_350 = goertzel_result(&dsp->freqs[HZ_350]);
01104          hz_440 = goertzel_result(&dsp->freqs[HZ_440]);
01105          hz_480 = goertzel_result(&dsp->freqs[HZ_480]);
01106          hz_620 = goertzel_result(&dsp->freqs[HZ_620]);
01107          hz_950 = goertzel_result(&dsp->freqs[HZ_950]);
01108          hz_1400 = goertzel_result(&dsp->freqs[HZ_1400]);
01109          hz_1800 = goertzel_result(&dsp->freqs[HZ_1800]);
01110 #if 0
01111          printf("Got whole dsp state: 350: %e, 440: %e, 480: %e, 620: %e, 950: %e, 1400: %e, 1800: %e, Energy: %e\n", 
01112             hz_350, hz_440, hz_480, hz_620, hz_950, hz_1400, hz_1800, dsp->genergy);
01113 #endif
01114          if (pair_there(hz_480, hz_620, hz_350, hz_440, dsp->genergy)) {
01115             newstate = TONE_STATE_BUSY;
01116          } else if (pair_there(hz_440, hz_480, hz_350, hz_620, dsp->genergy)) {
01117             newstate = TONE_STATE_RINGING;
01118          } else if (pair_there(hz_350, hz_440, hz_480, hz_620, dsp->genergy)) {
01119             newstate = TONE_STATE_DIALTONE;
01120          } else if (hz_950 > TONE_MIN_THRESH * TONE_THRESH) {
01121             newstate = TONE_STATE_SPECIAL1;
01122          } else if (hz_1400 > TONE_MIN_THRESH * TONE_THRESH) {
01123             if (dsp->tstate == TONE_STATE_SPECIAL1)
01124                newstate = TONE_STATE_SPECIAL2;
01125          } else if (hz_1800 > TONE_MIN_THRESH * TONE_THRESH) {
01126             if (dsp->tstate == TONE_STATE_SPECIAL2)
01127                newstate = TONE_STATE_SPECIAL3;
01128          } else if (dsp->genergy > TONE_MIN_THRESH * TONE_THRESH) {
01129             newstate = TONE_STATE_TALKING;
01130          } else
01131             newstate = TONE_STATE_SILENCE;
01132          
01133          if (newstate == dsp->tstate) {
01134             dsp->tcount++;
01135             if (dsp->tcount == COUNT_THRESH) {
01136                if (dsp->tstate == TONE_STATE_BUSY) {
01137                   res = AST_CONTROL_BUSY;
01138                   dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
01139                } else if (dsp->tstate == TONE_STATE_TALKING) {
01140                   res = AST_CONTROL_ANSWER;
01141                   dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
01142                } else if (dsp->tstate == TONE_STATE_RINGING)
01143                   res = AST_CONTROL_RINGING;
01144                else if (dsp->tstate == TONE_STATE_SPECIAL3) {
01145                   res = AST_CONTROL_CONGESTION;
01146                   dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
01147                }
01148                
01149             }
01150          } else {
01151 #if 0
01152             printf("Newstate: %d\n", newstate);
01153 #endif
01154             dsp->tstate = newstate;
01155             dsp->tcount = 1;
01156          }
01157          
01158          /* Reset goertzel */                
01159          for (x=0;x<7;x++)
01160             dsp->freqs[x].v2 = dsp->freqs[x].v3 = 0.0;
01161          dsp->gsamps = 0;
01162          dsp->genergy = 0.0;
01163       }
01164    }
01165 #if 0
01166    if (res)
01167       printf("Returning %d\n", res);
01168 #endif      
01169    return res;
01170 }
01171 
01172 int ast_dsp_call_progress(struct ast_dsp *dsp, struct ast_frame *inf)
01173 {
01174    if (inf->frametype != AST_FRAME_VOICE) {
01175       ast_log(LOG_WARNING, "Can't check call progress of non-voice frames\n");
01176       return 0;
01177    }
01178    if (inf->subclass != AST_FORMAT_SLINEAR) {
01179       ast_log(LOG_WARNING, "Can only check call progress in signed-linear frames\n");
01180       return 0;
01181    }
01182    return __ast_dsp_call_progress(dsp, inf->data, inf->datalen / 2);
01183 }
01184 
01185 static int __ast_dsp_silence(struct ast_dsp *dsp, short *s, int len, int *totalsilence)
01186 {
01187    int accum;
01188    int x;
01189    int res = 0;
01190    
01191    accum = 0;
01192    for (x=0;x<len; x++) 
01193       accum += abs(s[x]);
01194    accum /= len;
01195    if (accum < dsp->threshold) {
01196       dsp->totalsilence += len/8;
01197       if (dsp->totalnoise) {
01198          /* Move and save history */
01199          memmove(dsp->historicnoise + DSP_HISTORY - dsp->busycount, dsp->historicnoise + DSP_HISTORY - dsp->busycount +1, dsp->busycount*sizeof(dsp->historicnoise[0]));
01200          dsp->historicnoise[DSP_HISTORY - 1] = dsp->totalnoise;
01201 /* we don't want to check for busydetect that frequently */
01202 #if 0
01203          dsp->busymaybe = 1;
01204 #endif
01205       }
01206       dsp->totalnoise = 0;
01207       res = 1;
01208    } else {
01209       dsp->totalnoise += len/8;
01210       if (dsp->totalsilence) {
01211          int silence1 = dsp->historicsilence[DSP_HISTORY - 1];
01212          int silence2 = dsp->historicsilence[DSP_HISTORY - 2];
01213          /* Move and save history */
01214          memmove(dsp->historicsilence + DSP_HISTORY - dsp->busycount, dsp->historicsilence + DSP_HISTORY - dsp->busycount + 1, dsp->busycount*sizeof(dsp->historicsilence[0]));
01215          dsp->historicsilence[DSP_HISTORY - 1] = dsp->totalsilence;
01216          /* check if the previous sample differs only by BUSY_PERCENT from the one before it */
01217          if (silence1 < silence2) {
01218             if (silence1 + silence1/BUSY_PERCENT >= silence2)
01219                dsp->busymaybe = 1;
01220             else 
01221                dsp->busymaybe = 0;
01222          } else {
01223             if (silence1 - silence1/BUSY_PERCENT <= silence2)
01224                dsp->busymaybe = 1;
01225             else 
01226                dsp->busymaybe = 0;
01227          }
01228                
01229       }
01230       dsp->totalsilence = 0;
01231    }
01232    if (totalsilence)
01233       *totalsilence = dsp->totalsilence;
01234    return res;
01235 }
01236 #ifdef BUSYDETECT_MARTIN
01237 int ast_dsp_busydetect(struct ast_dsp *dsp)
01238 {
01239    int res = 0, x;
01240 #ifndef BUSYDETECT_TONEONLY
01241    int avgsilence = 0, hitsilence = 0;
01242 #endif
01243    int avgtone = 0, hittone = 0;
01244    if (!dsp->busymaybe)
01245       return res;
01246    for (x=DSP_HISTORY - dsp->busycount;x<DSP_HISTORY;x++) {
01247 #ifndef BUSYDETECT_TONEONLY
01248       avgsilence += dsp->historicsilence[x];
01249 #endif
01250       avgtone += dsp->historicnoise[x];
01251    }
01252 #ifndef BUSYDETECT_TONEONLY
01253    avgsilence /= dsp->busycount;
01254 #endif
01255    avgtone /= dsp->busycount;
01256    for (x=DSP_HISTORY - dsp->busycount;x<DSP_HISTORY;x++) {
01257 #ifndef BUSYDETECT_TONEONLY
01258       if (avgsilence > dsp->historicsilence[x]) {
01259          if (avgsilence - (avgsilence / BUSY_PERCENT) <= dsp->historicsilence[x])
01260             hitsilence++;
01261       } else {
01262          if (avgsilence + (avgsilence / BUSY_PERCENT) >= dsp->historicsilence[x])
01263             hitsilence++;
01264       }
01265 #endif
01266       if (avgtone > dsp->historicnoise[x]) {
01267          if (avgtone - (avgtone / BUSY_PERCENT) <= dsp->historicsilence[x])
01268             hittone++;
01269       } else {
01270          if (avgtone + (avgtone / BUSY_PERCENT) >= dsp->historicsilence[x])
01271             hittone++;
01272       }
01273    }
01274 #ifndef BUSYDETECT_TONEONLY
01275    if ((hittone >= dsp->busycount - 1) && (hitsilence >= dsp->busycount - 1) && (avgtone >= BUSY_MIN && avgtone <= BUSY_MAX) && (avgsilence >= BUSY_MIN && avgsilence <= BUSY_MAX)) {
01276 #else
01277    if ((hittone >= dsp->busycount - 1) && (avgtone >= BUSY_MIN && avgtone <= BUSY_MAX)) {
01278 #endif
01279 #ifdef BUSYDETECT_COMPARE_TONE_AND_SILENCE
01280 #ifdef BUSYDETECT_TONEONLY
01281 #error You cant use BUSYDETECT_TONEONLY together with BUSYDETECT_COMPARE_TONE_AND_SILENCE
01282 #endif
01283       if (avgtone > avgsilence) {
01284          if (avgtone - avgtone/(BUSY_PERCENT*2) <= avgsilence)
01285             res = 1;
01286       } else {
01287          if (avgtone + avgtone/(BUSY_PERCENT*2) >= avgsilence)
01288             res = 1;
01289       }
01290 #else
01291       res = 1;
01292 #endif
01293    }
01294 #if 0
01295    if (res)
01296       ast_log(LOG_NOTICE, "detected busy, avgtone: %d, avgsilence %d\n", avgtone, avgsilence);
01297 #endif
01298    return res;
01299 }
01300 #endif
01301 
01302 #ifdef BUSYDETECT
01303 int ast_dsp_busydetect(struct ast_dsp *dsp)
01304 {
01305    int x;
01306    int res = 0;
01307    int max, min;
01308 
01309 #if 0
01310    if (dsp->busy_hits > 5);
01311    return 0;
01312 #endif
01313    if (dsp->busymaybe) {
01314 #if 0
01315       printf("Maybe busy!\n");
01316 #endif      
01317       dsp->busymaybe = 0;
01318       min = 9999;
01319       max = 0;
01320       for (x=DSP_HISTORY - dsp->busycount;x<DSP_HISTORY;x++) {
01321 #if 0
01322          printf("Silence: %d, Noise: %d\n", dsp->historicsilence[x], dsp->historicnoise[x]);
01323 #endif         
01324          if (dsp->historicsilence[x] < min)
01325             min = dsp->historicsilence[x];
01326          if (dsp->historicnoise[x] < min)
01327             min = dsp->historicnoise[x];
01328          if (dsp->historicsilence[x] > max)
01329             max = dsp->historicsilence[x];
01330          if (dsp->historicnoise[x] > max)
01331             max = dsp->historicnoise[x];
01332       }
01333       if ((max - min < BUSY_THRESHOLD) && (max < BUSY_MAX) && (min > BUSY_MIN)) {
01334 #if 0
01335          printf("Busy!\n");
01336 #endif         
01337          res = 1;
01338       }
01339 #if 0
01340       printf("Min: %d, max: %d\n", min, max);
01341 #endif      
01342    }
01343    return res;
01344 }
01345 #endif
01346 
01347 int ast_dsp_silence(struct ast_dsp *dsp, struct ast_frame *f, int *totalsilence)
01348 {
01349    short *s;
01350    int len;
01351    
01352    if (f->frametype != AST_FRAME_VOICE) {
01353       ast_log(LOG_WARNING, "Can't calculate silence on a non-voice frame\n");
01354       return 0;
01355    }
01356    if (f->subclass != AST_FORMAT_SLINEAR) {
01357       ast_log(LOG_WARNING, "Can only calculate silence on signed-linear frames :(\n");
01358       return 0;
01359    }
01360    s = f->data;
01361    len = f->datalen/2;
01362    return __ast_dsp_silence(dsp, s, len, totalsilence);
01363 }
01364 
01365 struct ast_frame *ast_dsp_process(struct ast_channel *chan, struct ast_dsp *dsp, struct ast_frame *af, int needlock)
01366 {
01367    int silence;
01368    int res;
01369    int digit;
01370    int x;
01371    unsigned short *shortdata;
01372    unsigned char *odata;
01373    int len;
01374    int writeback = 0;
01375 
01376 #define FIX_INF(inf) do { \
01377       if (writeback) { \
01378          switch(inf->subclass) { \
01379          case AST_FORMAT_SLINEAR: \
01380             break; \
01381          case AST_FORMAT_ULAW: \
01382             for (x=0;x<len;x++) \
01383                odata[x] = AST_LIN2MU(shortdata[x]); \
01384             break; \
01385          case AST_FORMAT_ALAW: \
01386             for (x=0;x<len;x++) \
01387                odata[x] = AST_LIN2A(shortdata[x]); \
01388             break; \
01389          } \
01390       } \
01391    } while(0) 
01392 
01393    if (!af)
01394       return NULL;
01395    if (af->frametype != AST_FRAME_VOICE)
01396       return af;
01397    odata = af->data;
01398    len = af->datalen;
01399    /* Make sure we have short data */
01400    switch(af->subclass) {
01401    case AST_FORMAT_SLINEAR:
01402       shortdata = af->data;
01403       len = af->datalen / 2;
01404       break;
01405    case AST_FORMAT_ULAW:
01406       shortdata = alloca(af->datalen * 2);
01407       if (!shortdata) {
01408          ast_log(LOG_WARNING, "Unable to allocate stack space for data: %s\n", strerror(errno));
01409          return af;
01410       }
01411       for (x=0;x<len;x++) 
01412          shortdata[x] = AST_MULAW(odata[x]);
01413       break;
01414    case AST_FORMAT_ALAW:
01415       shortdata = alloca(af->datalen * 2);
01416       if (!shortdata) {
01417          ast_log(LOG_WARNING, "Unable to allocate stack space for data: %s\n", strerror(errno));
01418          return af;
01419       }
01420       for (x=0;x<len;x++) 
01421          shortdata[x] = AST_ALAW(odata[x]);
01422       break;
01423    default:
01424       ast_log(LOG_WARNING, "Unable to process inband DTMF on %d frames\n", af->subclass);
01425       return af;
01426    }
01427    silence = __ast_dsp_silence(dsp, shortdata, len, NULL);
01428    if ((dsp->features & DSP_FEATURE_SILENCE_SUPPRESS) && silence) {
01429       memset(&dsp->f, 0, sizeof(dsp->f));
01430       dsp->f.frametype = AST_FRAME_NULL;
01431       return &dsp->f;
01432    }
01433    if ((dsp->features & DSP_FEATURE_BUSY_DETECT) && ast_dsp_busydetect(dsp)) {
01434       chan->_softhangup |= AST_SOFTHANGUP_DEV;
01435       memset(&dsp->f, 0, sizeof(dsp->f));
01436       dsp->f.frametype = AST_FRAME_CONTROL;
01437       dsp->f.subclass = AST_CONTROL_BUSY;
01438       ast_log(LOG_DEBUG, "Requesting Hangup because the busy tone was detected on channel %s\n", chan->name);
01439       return &dsp->f;
01440    }
01441    if ((dsp->features & DSP_FEATURE_DTMF_DETECT)) {
01442       digit = __ast_dsp_digitdetect(dsp, shortdata, len, &writeback);
01443 #if 0
01444       if (digit)
01445          printf("Performing digit detection returned %d, digitmode is %d\n", digit, dsp->digitmode);
01446 #endif         
01447       if (dsp->digitmode & (DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX)) {
01448          if (!dsp->thinkdigit) {
01449             if (digit) {
01450                /* Looks like we might have something.  Request a conference mute for the moment */
01451                memset(&dsp->f, 0, sizeof(dsp->f));
01452                dsp->f.frametype = AST_FRAME_DTMF;
01453                dsp->f.subclass = 'm';
01454                dsp->thinkdigit = 'x';
01455                FIX_INF(af);
01456                if (chan)
01457                   ast_queue_frame(chan, af, needlock);
01458                ast_frfree(af);
01459                return &dsp->f;
01460             }
01461          } else {
01462             if (digit) {
01463                /* Thought we saw one last time.  Pretty sure we really have now */
01464                if (dsp->thinkdigit) {
01465                   if ((dsp->thinkdigit != 'x') && (dsp->thinkdigit != digit)) {
01466                      /* If we found a digit, and we're changing digits, go
01467                         ahead and send this one, but DON'T stop confmute because
01468                         we're detecting something else, too... */
01469                      memset(&dsp->f, 0, sizeof(dsp->f));
01470                      dsp->f.frametype = AST_FRAME_DTMF;
01471                      dsp->f.subclass = dsp->thinkdigit;
01472                      FIX_INF(af);
01473                      if (chan)
01474                         ast_queue_frame(chan, af, needlock);
01475                      ast_frfree(af);
01476                   }
01477                   dsp->thinkdigit = digit;
01478                   return &dsp->f;
01479                }
01480                dsp->thinkdigit = digit;
01481             } else {
01482                if (dsp->thinkdigit) {
01483                   memset(&dsp->f, 0, sizeof(dsp->f));
01484                   if (dsp->thinkdigit != 'x') {
01485                      /* If we found a digit, send it now */
01486                      dsp->f.frametype = AST_FRAME_DTMF;
01487                      dsp->f.subclass = dsp->thinkdigit;
01488                      dsp->thinkdigit = 0;
01489                   } else {
01490                      dsp->f.frametype = AST_FRAME_DTMF;
01491                      dsp->f.subclass = 'u';
01492                      dsp->thinkdigit = 0;
01493                   }
01494                   FIX_INF(af);
01495                   if (chan)
01496                      ast_queue_frame(chan, af, needlock);
01497                   ast_frfree(af);
01498                   return &dsp->f;
01499                }
01500             }
01501          }
01502       } else if (!digit) {
01503          /* Only check when there is *not* a hit... */
01504          if (dsp->digitmode & DSP_DIGITMODE_MF) {
01505             if (dsp->td.mf.current_digits) {
01506                memset(&dsp->f, 0, sizeof(dsp->f));
01507                dsp->f.frametype = AST_FRAME_DTMF;
01508                dsp->f.subclass = dsp->td.mf.digits[0];
01509                memmove(dsp->td.mf.digits, dsp->td.mf.digits + 1, dsp->td.mf.current_digits);
01510                dsp->td.mf.current_digits--;
01511                FIX_INF(af);
01512                if (chan)
01513                   ast_queue_frame(chan, af, needlock);
01514                ast_frfree(af);
01515                return &dsp->f;
01516             }
01517          } else {
01518             if (dsp->td.dtmf.current_digits) {
01519                memset(&dsp->f, 0, sizeof(dsp->f));
01520                dsp->f.frametype = AST_FRAME_DTMF;
01521                dsp->f.subclass = dsp->td.dtmf.digits[0];
01522                memmove(dsp->td.dtmf.digits, dsp->td.dtmf.digits + 1, dsp->td.dtmf.current_digits);
01523                dsp->td.dtmf.current_digits--;
01524                FIX_INF(af);
01525                if (chan)
01526                   ast_queue_frame(chan, af, needlock);
01527                ast_frfree(af);
01528                return &dsp->f;
01529             }
01530          }
01531       }
01532    }
01533    if ((dsp->features & DSP_FEATURE_CALL_PROGRESS)) {
01534       res = __ast_dsp_call_progress(dsp, shortdata, len);
01535       memset(&dsp->f, 0, sizeof(dsp->f));
01536       dsp->f.frametype = AST_FRAME_CONTROL;
01537       if (res) {
01538          switch(res) {
01539          case AST_CONTROL_ANSWER:
01540          case AST_CONTROL_BUSY:
01541          case AST_CONTROL_RINGING:
01542          case AST_CONTROL_CONGESTION:
01543             dsp->f.subclass = res;
01544             if (chan) 
01545                ast_queue_frame(chan, &dsp->f, needlock);
01546             break;
01547          default:
01548             ast_log(LOG_WARNING, "Don't know how to represent call progress message %d\n", res);
01549          }
01550       }
01551    }
01552    FIX_INF(af);
01553    return af;
01554 }
01555 
01556 struct ast_dsp *ast_dsp_new(void)
01557 {
01558    struct ast_dsp *dsp;
01559    dsp = malloc(sizeof(struct ast_dsp));
01560    if (dsp) {
01561       memset(dsp, 0, sizeof(struct ast_dsp));
01562       dsp->threshold = DEFAULT_THRESHOLD;
01563       dsp->features = DSP_FEATURE_SILENCE_SUPPRESS;
01564       dsp->busycount = DSP_HISTORY;
01565       /* Initialize goertzels */
01566       goertzel_init(&dsp->freqs[HZ_350], 350.0, GSAMP_SIZE);
01567       goertzel_init(&dsp->freqs[HZ_440], 440.0, GSAMP_SIZE);
01568       goertzel_init(&dsp->freqs[HZ_480], 480.0, GSAMP_SIZE);
01569       goertzel_init(&dsp->freqs[HZ_620], 620.0, GSAMP_SIZE);
01570       goertzel_init(&dsp->freqs[HZ_950], 950.0, GSAMP_SIZE);
01571       goertzel_init(&dsp->freqs[HZ_1400], 1400.0, GSAMP_SIZE);
01572       goertzel_init(&dsp->freqs[HZ_1800], 1800.0, GSAMP_SIZE);
01573       /* Initialize DTMF detector */
01574       ast_dtmf_detect_init(&dsp->td.dtmf);
01575    }
01576    return dsp;
01577 }
01578 
01579 void ast_dsp_set_features(struct ast_dsp *dsp, int features)
01580 {
01581    dsp->features = features;
01582 }
01583 
01584 void ast_dsp_free(struct ast_dsp *dsp)
01585 {
01586    free(dsp);
01587 }
01588 
01589 void ast_dsp_set_threshold(struct ast_dsp *dsp, int threshold)
01590 {
01591    dsp->threshold = threshold;
01592 }
01593 
01594 void ast_dsp_set_busy_count(struct ast_dsp *dsp, int cadences)
01595 {
01596    if (cadences < 4)
01597       cadences = 4;
01598    if (cadences > DSP_HISTORY)
01599       cadences = DSP_HISTORY;
01600    dsp->busycount = cadences;
01601 }
01602 
01603 void ast_dsp_digitreset(struct ast_dsp *dsp)
01604 {
01605    int i;
01606    dsp->thinkdigit = 0;
01607    if (dsp->digitmode & DSP_DIGITMODE_MF) {
01608       memset(dsp->td.mf.digits, 0, sizeof(dsp->td.mf.digits));
01609       dsp->td.mf.current_digits = 0;
01610       /* Reinitialise the detector for the next block */
01611       for (i = 0;  i < 6;  i++) {
01612             goertzel_reset(&dsp->td.mf.tone_out[i]);
01613 #ifdef OLD_DSP_ROUTINES
01614           goertzel_reset(&dsp->td.mf.tone_out2nd[i]);
01615 #endif         
01616       }
01617 #ifdef OLD_DSP_ROUTINES
01618       dsp->td.mf.energy = 0.0;
01619        dsp->td.mf.hit1 = dsp->td.mf.hit2 = dsp->td.mf.hit3 = dsp->td.mf.hit4 = dsp->td.mf.mhit = 0;
01620 #else
01621        dsp->td.mf.hits[4] = dsp->td.mf.hits[3] = dsp->td.mf.hits[2] = dsp->td.mf.hits[1] = dsp->td.mf.hits[0] = dsp->td.mf.mhit = 0;
01622 #endif      
01623       dsp->td.mf.current_sample = 0;
01624    } else {
01625       memset(dsp->td.dtmf.digits, 0, sizeof(dsp->td.dtmf.digits));
01626       dsp->td.dtmf.current_digits = 0;
01627       /* Reinitialise the detector for the next block */
01628       for (i = 0;  i < 4;  i++) {
01629             goertzel_reset(&dsp->td.dtmf.row_out[i]);
01630           goertzel_reset(&dsp->td.dtmf.col_out[i]);
01631 #ifdef OLD_DSP_ROUTINES
01632          goertzel_reset(&dsp->td.dtmf.row_out2nd[i]);
01633          goertzel_reset(&dsp->td.dtmf.col_out2nd[i]);
01634 #endif         
01635       }
01636        goertzel_reset (&dsp->td.dtmf.fax_tone);
01637 #ifdef OLD_DSP_ROUTINES
01638        goertzel_reset (&dsp->td.dtmf.fax_tone2nd);
01639        dsp->td.dtmf.hit1 = dsp->td.dtmf.hit2 = dsp->td.dtmf.hit3 = dsp->td.dtmf.hit4 = dsp->td.dtmf.mhit = 0;
01640 #else
01641        dsp->td.dtmf.hits[2] = dsp->td.dtmf.hits[1] = dsp->td.dtmf.hits[0] =  dsp->td.dtmf.mhit = 0;
01642 #endif      
01643       dsp->td.dtmf.energy = 0.0;
01644       dsp->td.dtmf.current_sample = 0;
01645    }
01646 }
01647 
01648 void ast_dsp_reset(struct ast_dsp *dsp)
01649 {
01650    int x;
01651    dsp->totalsilence = 0;
01652    dsp->gsamps = 0;
01653    for (x=0;x<4;x++)
01654       dsp->freqs[x].v2 = dsp->freqs[x].v3 = 0.0;
01655    memset(dsp->historicsilence, 0, sizeof(dsp->historicsilence));
01656    memset(dsp->historicnoise, 0, sizeof(dsp->historicnoise));
01657    
01658 }
01659 
01660 int ast_dsp_digitmode(struct ast_dsp *dsp, int digitmode)
01661 {
01662    int new, old;
01663    old = dsp->digitmode & (DSP_DIGITMODE_DTMF | DSP_DIGITMODE_MF | DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX);
01664    new = digitmode & (DSP_DIGITMODE_DTMF | DSP_DIGITMODE_MF | DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX);
01665    if (old != new) {
01666       /* Must initialize structures if switching from MF to DTMF or vice-versa */
01667       if (new & DSP_DIGITMODE_MF)
01668          ast_mf_detect_init(&dsp->td.mf);
01669       else
01670          ast_dtmf_detect_init(&dsp->td.dtmf);
01671    }
01672    dsp->digitmode = digitmode;
01673    return 0;
01674 }
01675 

Generated on Fri Feb 27 12:19:42 2004 for Asterisk by doxygen 1.3.5