sjero.net Git - linphone/blob - linphone/mediastreamer2/src/msvolume.c

   1 /*
   2 mediastreamer2 library - modular sound and video processing and streaming
   3 Copyright (C) 2006  Simon MORLAT (simon.morlat@linphone.org)
   4
   5 This program is free software; you can redistribute it and/or
   6 modify it under the terms of the GNU General Public License
   7 as published by the Free Software Foundation; either version 2
   8 of the License, or (at your option) any later version.
   9
  10 This program is distributed in the hope that it will be useful,
  11 but WITHOUT ANY WARRANTY; without even the implied warranty of
  12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13 GNU General Public License for more details.
  14
  15 You should have received a copy of the GNU General Public License
  16 along with this program; if not, write to the Free Software
  17 Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  18 */
  19
  20 #ifdef HAVE_CONFIG_H
  21 #include "mediastreamer-config.h"
  22 #endif
  23
  24 #include "mediastreamer2/msvolume.h"
  25 #include "mediastreamer2/msticker.h"
  26 #include <math.h>
  27
  28 #ifdef HAVE_SPEEXDSP
  29 #include <speex/speex_preprocess.h>
  30 #endif
  31
  32 static const float max_e=32767*32767;
  33 static const float coef=0.1;
  34 static const float gain_k=0.02;
  35 static const float en_weight=4.0;
  36 static const float noise_thres=0.1;
  37
  38
  39 typedef struct Volume{
  40         float energy;
  41         float norm_en;
  42         float gain; /*the one really applied, smoothed by noise gate and echo limiter*/
  43         float static_gain; /*the one fixed by the user*/
  44         float gain_k;
  45         float thres;
  46         float force;
  47         float target_gain; /*the target gain choosed by echo limiter and noise gate*/
  48         float last_peer_en;
  49         int sustain_time; /* time in ms for which echo limiter remains active after resuming from speech to silence.*/
  50         uint64_t sustain_start;
  51         MSFilter *peer;
  52 #ifdef HAVE_SPEEXDSP
  53         SpeexPreprocessState *speex_pp;
  54 #endif
  55         int sample_rate;
  56         int nsamples;
  57         int ng_cut_time; /*noise gate cut time, after last speech detected*/
  58         int ng_noise_dur;
  59         float ng_threshold;
  60         float ng_floorgain;
  61         MSBufferizer *buffer;
  62         bool_t ea_active;
  63         bool_t agc_enabled;
  64         bool_t noise_gate_enabled;
  65 }Volume;
  66
  67 static void volume_init(MSFilter *f){
  68         Volume *v=(Volume*)ms_new(Volume,1);
  69         v->energy=0;
  70         v->norm_en=0;
  71         v->static_gain=v->gain=1;
  72         v->ea_active=FALSE;
  73         v->gain_k=gain_k;
  74         v->thres=noise_thres;
  75         v->force=en_weight;
  76         v->peer=NULL;
  77         v->last_peer_en=0;
  78         v->sustain_time=200;
  79         v->sustain_start=0;
  80         v->agc_enabled=FALSE;
  81         v->buffer=ms_bufferizer_new();
  82         v->sample_rate=8000;
  83         v->nsamples=80;
  84         v->noise_gate_enabled=FALSE;
  85         v->ng_cut_time=100;/*milliseconds*/
  86         v->ng_noise_dur=0;
  87         v->ng_threshold=noise_thres;
  88         v->ng_floorgain=0;
  89 #ifdef HAVE_SPEEXDSP
  90         v->speex_pp=NULL;
  91 #endif
  92         f->data=v;
  93 }
  94
  95 static void volume_uninit(MSFilter *f){
  96         Volume *v=(Volume*)f->data;
  97 #ifdef HAVE_SPEEXDSP
  98         if (v->speex_pp)
  99                 speex_preprocess_state_destroy(v->speex_pp);
 100 #endif
 101         ms_bufferizer_destroy(v->buffer);
 102         ms_free(f->data);
 103 }
 104
 105 static int volume_get(MSFilter *f, void *arg){
 106         float *farg=(float*)arg;
 107         Volume *v=(Volume*)f->data;
 108         *farg=10*ortp_log10f((v->energy+1)/max_e);
 109
 110         return 0;
 111 }
 112
 113 static int volume_set_sample_rate(MSFilter *f, void *arg){
 114         Volume *v=(Volume*)f->data;
 115         v->sample_rate=*(int*)arg;
 116         return 0;
 117 }
 118
 119 static int volume_get_linear(MSFilter *f, void *arg){
 120         float *farg=(float*)arg;
 121         Volume *v=(Volume*)f->data;
 122         *farg=(v->energy+1)/max_e;
 123         return 0;
 124 }
 125 #ifdef HAVE_SPEEXDSP
 126 static void volume_agc_process(Volume *v, mblk_t *om){
 127         speex_preprocess_run(v->speex_pp,(int16_t*)om->b_rptr);
 128 }
 129 #else
 130
 131 static void volume_agc_process(Volume *v, mblk_t *om){
 132 }
 133
 134 #endif
 135
 136
 137 static inline float compute_gain(float static_gain, float energy, float weight){
 138         float ret=static_gain*(1 - (energy*weight));
 139         if (ret<0) ret=0;
 140         return ret;
 141 }
 142
 143 /*
 144 The principle of this algorithm is that we apply a gain to the input signal which is opposite to the
 145 energy measured by the peer MSVolume.
 146 For example if some noise is played by the speaker, then the signal captured by the microphone will be lowered.
 147 The gain changes smoothly when the peer energy is decreasing, but is immediately changed when the peer energy is
 148 increasing.
 149 */
 150
 151 static void volume_echo_avoider_process(Volume *v, uint64_t curtime){
 152         float peer_e;
 153         float gain;
 154         ms_filter_call_method(v->peer,MS_VOLUME_GET_LINEAR,&peer_e);
 155         peer_e=sqrt(peer_e);
 156         if (v->ea_active){
 157                 if (peer_e>v->thres){
 158                         /*lower our output*/
 159                         gain=compute_gain(v->static_gain,peer_e,v->force);
 160                         if (peer_e>v->last_peer_en)
 161                                 v->gain=gain;
 162                 }else {
 163                         v->sustain_start=curtime;
 164                         v->ea_active=FALSE;
 165                         gain=v->gain;
 166                 }
 167         }else{
 168                 int peer_active=FALSE;
 169                 ms_filter_call_method(v->peer,MS_VOLUME_GET_EA_STATE,&peer_active);
 170                 if (peer_e>v->thres && ! peer_active){
 171                         /*lower our output*/
 172                         gain=compute_gain(v->static_gain,peer_e,v->force);
 173                         v->ea_active=TRUE;
 174                         v->gain=gain;
 175                 }else {
 176                         if (curtime!=0 && (curtime-v->sustain_start)<v->sustain_time){
 177                                 gain=v->gain;
 178                         }else{/*restore normal gain*/
 179                                 gain=v->static_gain;
 180                                 v->sustain_start=0;
 181                         }
 182                 }
 183         }
 184         v->last_peer_en=peer_e;
 185         v->target_gain=gain;
 186         ms_message("ea_active=%i, peer_e=%f gain=%f gain_k=%f force=%f",v->ea_active,peer_e,v->gain, v->gain_k,v->force);
 187 }
 188
 189 static void volume_noise_gate_process(Volume *v , float energy, mblk_t *om){
 190         int nsamples=((om->b_wptr-om->b_rptr)/2);
 191         if ((energy/max_e)<v->ng_threshold){
 192                 v->ng_noise_dur+=(nsamples*1000)/v->sample_rate;
 193                 if (v->ng_noise_dur>v->ng_cut_time){
 194                         v->target_gain=v->ng_floorgain;
 195                 }
 196         }else{
 197                 v->ng_noise_dur=0;
 198                 /*let the target gain unchanged, ie let the echo-limiter choose the gain*/
 199         }
 200 }
 201
 202 static int volume_set_gain(MSFilter *f, void *arg){
 203         float *farg=(float*)arg;
 204         Volume *v=(Volume*)f->data;
 205         v->gain=v->static_gain=v->target_gain=*farg;
 206         return 0;
 207 }
 208
 209
 210 static int volume_get_ea_state(MSFilter *f, void *arg){
 211         int *barg=(int*)arg;
 212         Volume *v=(Volume*)f->data;
 213         *barg=v->ea_active;
 214         return 0;
 215 }
 216
 217 static int volume_set_peer(MSFilter *f, void *arg){
 218         MSFilter *p=(MSFilter*)arg;
 219         Volume *v=(Volume*)f->data;
 220         v->peer=p;
 221         return 0;
 222 }
 223
 224 static int volume_set_agc(MSFilter *f, void *arg){
 225         Volume *v=(Volume*)f->data;
 226         v->agc_enabled=*(int*)arg;
 227         return 0;
 228 }
 229
 230 static int volume_set_ea_threshold(MSFilter *f, void*arg){
 231         Volume *v=(Volume*)f->data;
 232         float val=*(float*)arg;
 233         if (val<0 || val>1) {
 234                 ms_error("Error: threshold must be in range [0..1]");
 235                 return -1;
 236         }
 237         v->thres=val;
 238         return 0;
 239 }
 240
 241 static int volume_set_ea_speed(MSFilter *f, void*arg){
 242         Volume *v=(Volume*)f->data;
 243         float val=*(float*)arg;
 244         if (val<0 || val>1) {
 245                 ms_error("Error: speed must be in range [0..1]");
 246                 return -1;
 247         }
 248         v->gain_k=val;
 249         return 0;
 250 }
 251
 252 static int volume_set_ea_force(MSFilter *f, void*arg){
 253         Volume *v=(Volume*)f->data;
 254         float val=*(float*)arg;
 255         v->force=val;
 256         return 0;
 257 }
 258
 259 static int volume_set_ea_sustain(MSFilter *f, void *arg){
 260         Volume *v=(Volume*)f->data;
 261         v->sustain_time=*(int*)arg;
 262         return 0;
 263 }
 264
 265 static int volume_enable_noise_gate(MSFilter *f, void *arg){
 266         Volume *v=(Volume*)f->data;
 267         v->noise_gate_enabled=*(int*)arg;
 268         return 0;
 269 }
 270
 271 static int volume_set_noise_gate_threshold(MSFilter *f, void *arg){
 272         Volume *v=(Volume*)f->data;
 273         v->ng_threshold=*(float*)arg;
 274         return 0;
 275 }
 276
 277 static int volume_set_noise_gate_floorgain(MSFilter *f, void *arg){
 278         Volume *v=(Volume*)f->data;
 279         v->ng_floorgain=*(float*)arg;
 280         return 0;
 281 }
 282
 283 static inline int16_t saturate(float val){
 284         return (val>32767) ? 32767 : ( (val<-32767) ? -32767 : val);
 285 }
 286
 287 static float update_energy(int16_t *signal, int numsamples, float last_energy_value){
 288         int i;
 289         float en=last_energy_value;
 290         for (i=0;i<numsamples;++i){
 291                 float s=(float)signal[i];
 292                 en=(s*s*coef) + (1.0-coef)*en;
 293         }
 294         return en;
 295 }
 296
 297 static void apply_gain(Volume *v, mblk_t *m){
 298         int16_t *sample;
 299         float gain=v->target_gain;
 300
 301         if (gain==1 && v->gain==1) return;
 302         v->gain=(v->gain*(1-v->gain_k)) + (v->gain_k*gain);
 303         for (   sample=(int16_t*)m->b_rptr;
 304                                 sample<(int16_t*)m->b_wptr;
 305                                 ++sample){
 306                 float s=*sample;
 307                 *sample=saturate(s*v->gain);
 308         }
 309 }
 310
 311 static void volume_preprocess(MSFilter *f){
 312         Volume *v=(Volume*)f->data;
 313         /*process agc by chunks of 10 ms*/
 314         v->nsamples=(int)(0.01*(float)v->sample_rate);
 315         if (v->agc_enabled){
 316                 ms_message("AGC is enabled.");
 317 #ifdef HAVE_SPEEXDSP
 318                 if (v->speex_pp==NULL){
 319                         int tmp=1;
 320                         v->speex_pp=speex_preprocess_state_init(v->nsamples,v->sample_rate);
 321                         if (speex_preprocess_ctl(v->speex_pp,SPEEX_PREPROCESS_SET_AGC,&tmp)==-1){
 322                                 ms_warning("Speex AGC is not available.");
 323                         }
 324                         tmp=0;
 325                         speex_preprocess_ctl(v->speex_pp,SPEEX_PREPROCESS_SET_VAD,&tmp);
 326                         speex_preprocess_ctl(v->speex_pp,SPEEX_PREPROCESS_SET_DENOISE,&tmp);
 327                         speex_preprocess_ctl(v->speex_pp,SPEEX_PREPROCESS_SET_DEREVERB,&tmp);
 328                 }
 329 #else
 330                 ms_error("No AGC possible, mediastreamer2 was compiled without libspeexdsp.");
 331 #endif
 332         }
 333 }
 334
 335
 336
 337 static void volume_process(MSFilter *f){
 338         mblk_t *m;
 339         Volume *v=(Volume*)f->data;
 340         float en=v->energy;
 341
 342         if (v->agc_enabled){
 343                 mblk_t *om;
 344                 int nbytes=v->nsamples*2;
 345                 ms_bufferizer_put_from_queue(v->buffer,f->inputs[0]);
 346                 while(ms_bufferizer_get_avail(v->buffer)>=nbytes){
 347                         om=allocb(nbytes,0);
 348                         ms_bufferizer_read(v->buffer,om->b_wptr,nbytes);
 349                         om->b_wptr+=nbytes;
 350                         en=update_energy((int16_t*)om->b_rptr,v->nsamples,en);
 351                         volume_agc_process(v,om);
 352
 353                         if (v->peer){
 354                                 volume_echo_avoider_process(v,f->ticker->time);
 355                         }else v->target_gain=v->static_gain;
 356
 357                         if (v->noise_gate_enabled)
 358                                 volume_noise_gate_process(v,en,om);
 359                         apply_gain(v,om);
 360                         ms_queue_put(f->outputs[0],om);
 361                 }
 362         }else{
 363                 /*light processing: no agc. Work in place in the input buffer*/
 364                 while((m=ms_queue_get(f->inputs[0]))!=NULL){
 365                         en=update_energy((int16_t*)m->b_rptr,(m->b_wptr-m->b_rptr)/2,en);
 366                         if (v->peer){
 367                                 volume_echo_avoider_process(v,f->ticker->time);
 368                         }else v->target_gain=v->static_gain;
 369
 370                         if (v->noise_gate_enabled)
 371                                 volume_noise_gate_process(v,en,m);
 372                         apply_gain(v,m);
 373                         ms_queue_put(f->outputs[0],m);
 374                 }
 375         }
 376         v->energy=en;
 377 }
 378
 379 static MSFilterMethod methods[]={
 380         {       MS_VOLUME_GET           ,       volume_get              },
 381         {       MS_VOLUME_GET_LINEAR    ,       volume_get_linear       },
 382         {       MS_VOLUME_SET_GAIN      ,       volume_set_gain         },
 383         {       MS_VOLUME_GET_EA_STATE  ,       volume_get_ea_state     },
 384         {       MS_VOLUME_SET_PEER      ,       volume_set_peer         },
 385         {       MS_VOLUME_SET_EA_THRESHOLD ,    volume_set_ea_threshold },
 386         {       MS_VOLUME_SET_EA_SPEED  ,       volume_set_ea_speed     },
 387         {       MS_VOLUME_SET_EA_FORCE  ,       volume_set_ea_force     },
 388         {       MS_VOLUME_SET_EA_SUSTAIN,       volume_set_ea_sustain   },
 389         {       MS_FILTER_SET_SAMPLE_RATE,      volume_set_sample_rate  },
 390         {       MS_VOLUME_ENABLE_AGC    ,       volume_set_agc          },
 391         {       MS_VOLUME_ENABLE_NOISE_GATE,    volume_enable_noise_gate},
 392         {       MS_VOLUME_SET_NOISE_GATE_THRESHOLD,     volume_set_noise_gate_threshold},
 393         {       MS_VOLUME_SET_NOISE_GATE_FLOORGAIN,     volume_set_noise_gate_floorgain},
 394         {       0                       ,       NULL                    }
 395 };
 396
 397 #ifndef _MSC_VER
 398 MSFilterDesc ms_volume_desc={
 399         .name="MSVolume",
 400         .text=N_("A filter that controls and measure sound volume"),
 401         .id=MS_VOLUME_ID,
 402         .category=MS_FILTER_OTHER,
 403         .ninputs=1,
 404         .noutputs=1,
 405         .init=volume_init,
 406         .uninit=volume_uninit,
 407         .preprocess=volume_preprocess,
 408         .process=volume_process,
 409         .methods=methods
 410 };
 411 #else
 412 MSFilterDesc ms_volume_desc={
 413         MS_VOLUME_ID,
 414         "MSVolume",
 415         N_("A filter that controls and measure sound volume"),
 416         MS_FILTER_OTHER,
 417         NULL,
 418         1,
 419         1,
 420         volume_init,
 421         volume_preprocess,
 422         volume_process,
 423         NULL,
 424         volume_uninit,
 425         methods
 426 };
 427 #endif
 428
 429 MS_FILTER_DESC_EXPORT(ms_volume_desc)
 430
 431