declip.c 9.8 KB
Newer Older
Monty Montgomery's avatar
 
Monty Montgomery committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26
/*
 *
 *  postfish
 *    
 *      Copyright (C) 2002-2004 Monty and Xiph.Org
 *
 *  Postfish is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2, or (at your option)
 *  any later version.
 *   
 *  Postfish is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *   
 *  You should have received a copy of the GNU General Public License
 *  along with Postfish; see the file COPYING.  If not, write to the
 *  Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 * 
 */

#include "postfish.h"
#include <math.h>
#include <sys/types.h>
Monty Montgomery's avatar
 
Monty Montgomery committed
27
#include <fftw3.h>
Monty Montgomery's avatar
 
Monty Montgomery committed
28
#include "reconstruct.h"
Monty Montgomery's avatar
 
Monty Montgomery committed
29
#include "feedback.h"
Monty Montgomery's avatar
 
Monty Montgomery committed
30 31 32 33

extern int input_rate;
extern int input_ch;
extern int input_size;
Monty Montgomery's avatar
 
Monty Montgomery committed
34
extern int inbytes;
Monty Montgomery's avatar
 
Monty Montgomery committed
35 36

/* accessed only in playback thread/setup */
Monty Montgomery's avatar
 
Monty Montgomery committed
37 38 39 40
static fftwf_plan fftwf_weight;
static float *work;
static float *freq;

Monty Montgomery's avatar
 
Monty Montgomery committed
41 42 43
static int blocksize=0;
static int lopad=0,hipad=0;
static u_int32_t *widthlookup=0;
Monty Montgomery's avatar
 
Monty Montgomery committed
44 45 46 47
static float *window=0;
static float width=.5;
static float **lap=0;
static float **cache;
Monty Montgomery's avatar
 
Monty Montgomery committed
48 49 50 51 52 53 54
static int cache_samples;
static int fillstate=0; /* 0: uninitialized
			   1: normal
			   2: eof processed */
static time_linkage out;

/* accessed across threads */
Monty Montgomery's avatar
 
Monty Montgomery committed
55
sig_atomic_t declip_active=0;
Monty Montgomery's avatar
 
Monty Montgomery committed
56
sig_atomic_t declip_visible=0;
Monty Montgomery's avatar
 
Monty Montgomery committed
57 58
sig_atomic_t declip_converge=2; /* 0=over, 1=full, 2=half, 3=partial, 4=approx */

Monty Montgomery's avatar
 
Monty Montgomery committed
59
static float *chtrigger=0;
Monty Montgomery's avatar
 
Monty Montgomery committed
60
static sig_atomic_t pending_blocksize=0;
Monty Montgomery's avatar
 
Monty Montgomery committed
61 62
static float convergence=0.;
static float iterations=0.;
Monty Montgomery's avatar
 
Monty Montgomery committed
63 64 65 66 67


/* feedback! */
typedef struct declip_feedback{
  feedback_generic parent_class;
Monty Montgomery's avatar
 
Monty Montgomery committed
68
  float *peak;
Monty Montgomery's avatar
 
Monty Montgomery committed
69 70 71 72 73 74 75 76 77
  int *clipcount;
  int total;
} declip_feedback;

static feedback_generic_pool feedpool;

static feedback_generic *new_declip_feedback(void){
  declip_feedback *ret=malloc(sizeof(*ret));
  ret->clipcount=malloc((input_ch)*sizeof(*ret->clipcount));
Monty Montgomery's avatar
 
Monty Montgomery committed
78
  ret->peak=malloc((input_ch)*sizeof(*ret->peak));
Monty Montgomery's avatar
 
Monty Montgomery committed
79 80 81
  return (feedback_generic *)ret;
}

Monty Montgomery's avatar
 
Monty Montgomery committed
82
static void push_declip_feedback(int *clip,float *peak,int total){
83
  int n=input_ch;
Monty Montgomery's avatar
 
Monty Montgomery committed
84 85 86
  declip_feedback *f=(declip_feedback *)
    feedback_new(&feedpool,new_declip_feedback);
  memcpy(f->clipcount,clip,n*sizeof(*clip));
Monty Montgomery's avatar
 
Monty Montgomery committed
87
  memcpy(f->peak,peak,n*sizeof(*peak));
Monty Montgomery's avatar
 
Monty Montgomery committed
88 89 90 91
  f->total=total;
  feedback_push(&feedpool,(feedback_generic *)f);
}

Monty Montgomery's avatar
 
Monty Montgomery committed
92
int pull_declip_feedback(int *clip,float *peak,int *total){
Monty Montgomery's avatar
 
Monty Montgomery committed
93 94 95 96 97
  declip_feedback *f=(declip_feedback *)feedback_pull(&feedpool);

  if(!f)return 0;

  if(clip)memcpy(clip,f->clipcount,sizeof(*clip)*input_ch);
Monty Montgomery's avatar
 
Monty Montgomery committed
98
  if(peak)memcpy(peak,f->peak,sizeof(*peak)*input_ch);
Monty Montgomery's avatar
 
Monty Montgomery committed
99 100 101 102 103
  if(total)*total=f->total;

  feedback_old(&feedpool,(feedback_generic *)f);
  return 1;
}
Monty Montgomery's avatar
 
Monty Montgomery committed
104 105 106 107 108

/* called only by initial setup */
int declip_load(void){
  int i;
  chtrigger=malloc(input_ch*sizeof(*chtrigger));
Monty Montgomery's avatar
 
Monty Montgomery committed
109 110
  for(i=0;i<input_ch;i++)
    chtrigger[i]=1.;
Monty Montgomery's avatar
 
Monty Montgomery committed
111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136
  
  out.size=input_size;
  out.channels=input_ch;
  out.rate=input_rate;
  out.data=malloc(input_ch*sizeof(*out.data));
  for(i=0;i<input_ch;i++)
    out.data[i]=malloc(input_size*sizeof(**out.data));

  fillstate=0;
  cache=malloc(input_ch*sizeof(*cache));
  for(i=0;i<input_ch;i++)
    cache[i]=malloc(input_size*sizeof(**cache));
  lap=malloc(input_ch*sizeof(*lap));
  for(i=0;i<input_ch;i++)
    lap[i]=malloc(input_size*sizeof(**lap));

  return(0);
}

int declip_setblock(int n){
  if(n<32)return -1;
  if(n>input_size*2)return -1;
  pending_blocksize=n;
  return 0;
}

Monty Montgomery's avatar
 
Monty Montgomery committed
137
int declip_settrigger(float trigger,int ch){
Monty Montgomery's avatar
 
Monty Montgomery committed
138
  if(ch<0 || ch>=input_ch)return -1;
Monty Montgomery's avatar
 
Monty Montgomery committed
139
  pthread_mutex_lock(&master_mutex);
Monty Montgomery's avatar
 
Monty Montgomery committed
140
  chtrigger[ch]=trigger-(1./(1<<(inbytes*8-1)))-(1./(1<<(inbytes*8-2)));
Monty Montgomery's avatar
 
Monty Montgomery committed
141
  pthread_mutex_unlock(&master_mutex);
Monty Montgomery's avatar
 
Monty Montgomery committed
142 143 144
  return 0;
}

Monty Montgomery's avatar
 
Monty Montgomery committed
145
int declip_setiterations(float it){
Monty Montgomery's avatar
 
Monty Montgomery committed
146 147 148
  pthread_mutex_lock(&master_mutex);
  iterations=it;
  pthread_mutex_unlock(&master_mutex);
149
  return 0;
Monty Montgomery's avatar
 
Monty Montgomery committed
150 151
}

Monty Montgomery's avatar
 
Monty Montgomery committed
152
int declip_setconvergence(float c){
Monty Montgomery's avatar
 
Monty Montgomery committed
153 154 155
  pthread_mutex_lock(&master_mutex);
  convergence=c;
  pthread_mutex_unlock(&master_mutex);
156
  return 0;
Monty Montgomery's avatar
 
Monty Montgomery committed
157 158 159 160 161 162
}

/* called only in playback thread */
int declip_reset(void){
  /* reset cached pipe state */
  fillstate=0;
Monty Montgomery's avatar
 
Monty Montgomery committed
163
  while(pull_declip_feedback(NULL,NULL,NULL));
Monty Montgomery's avatar
 
Monty Montgomery committed
164 165 166
  return 0;
}

Monty Montgomery's avatar
 
Monty Montgomery committed
167
static void sliding_bark_average(float *f,int n,float width){
168
  int i=0;
Monty Montgomery's avatar
 
Monty Montgomery committed
169 170
  float acc=0.,del=0.;
  float sec[hipad+1];
Monty Montgomery's avatar
 
Monty Montgomery committed
171 172 173

  memset(sec,0,sizeof(sec));

Monty Montgomery's avatar
 
Monty Montgomery committed
174
  for(i=0;i<n/2;i++){
Monty Montgomery's avatar
 
Monty Montgomery committed
175 176 177

    int hi=widthlookup[i]>>16;
    int lo=widthlookup[i]&(0xffff);
Monty Montgomery's avatar
 
Monty Montgomery committed
178
    float del=hypot(f[(i<<1)+1],f[i<<1])/(lo-hi);
Monty Montgomery's avatar
 
Monty Montgomery committed
179

Monty Montgomery's avatar
 
Monty Montgomery committed
180 181
    float hidel=del/((i-hi+lopad));
    float lodel=del/((lo-i-lopad));
Monty Montgomery's avatar
 
Monty Montgomery committed
182 183 184 185 186 187 188 189 190 191 192 193 194

    sec[hi]+=hidel;
    sec[i+lopad]-=hidel;
    sec[i+lopad]-=lodel;
    sec[lo]+=lodel;

  }

  for(i=0;i<lopad;i++){
    del+=sec[i];
    acc+=del;
  }

Monty Montgomery's avatar
 
Monty Montgomery committed
195 196
  for(i=0;i<n/2;i++){
    f[(i<<1)+1]=f[i<<1]=1./(acc*acc);
Monty Montgomery's avatar
 
Monty Montgomery committed
197 198
    del+=sec[i+lopad];
    acc+=del;
Monty Montgomery's avatar
 
Monty Montgomery committed
199
    
Monty Montgomery's avatar
 
Monty Montgomery committed
200
  }
Monty Montgomery's avatar
 
Monty Montgomery committed
201
  f[n+1]=f[n]=f[n-1];
Monty Montgomery's avatar
 
Monty Montgomery committed
202 203
}

Monty Montgomery's avatar
 
Monty Montgomery committed
204 205 206 207 208 209
/* work,freq are passed through the static buffer fftwf requires */
static void declip(float *lap,float *out,
		   int blocksize,float trigger,
		   float epsilon, float iteration,
		   int *runningtotal, int *runningcount,float *peak){
  float flag[blocksize*2];
Monty Montgomery's avatar
 
Monty Montgomery committed
210
  int    iterbound,i,j,count=0;
Monty Montgomery's avatar
 
Monty Montgomery committed
211
  
Monty Montgomery's avatar
 
Monty Montgomery committed
212
  for(i=blocksize/2;i<blocksize*3/2;i++){
Monty Montgomery's avatar
 
Monty Montgomery committed
213
    if(fabs(work[i])>*peak)*peak=fabs(work[i]);
Monty Montgomery's avatar
 
Monty Montgomery committed
214
    flag[i]=0.;
Monty Montgomery's avatar
 
Monty Montgomery committed
215
    if(work[i]>=trigger || work[i]<=-trigger){
Monty Montgomery's avatar
 
Monty Montgomery committed
216
      flag[i]=1.;
Monty Montgomery's avatar
 
Monty Montgomery committed
217 218 219 220
      count++;
    }
  }
  
Monty Montgomery's avatar
 
Monty Montgomery committed
221
  *runningtotal+=blocksize;
Monty Montgomery's avatar
 
Monty Montgomery committed
222
  *runningcount+=count;
Monty Montgomery's avatar
 
Monty Montgomery committed
223 224 225

  if(declip_active){

Monty Montgomery's avatar
 
Monty Montgomery committed
226 227 228
    for(i=0;i<blocksize/2;i++)flag[i]=0.;
    for(i=blocksize*3/2;i<blocksize*2;i++)flag[i]=0.;

Monty Montgomery's avatar
 
Monty Montgomery committed
229 230 231
    for(i=0;i<blocksize/2;i++)work[i]=0.;
    for(i=0;i<blocksize;i++)work[i+blocksize/2]*=window[i];
    for(i=blocksize*3/2;i<blocksize*2;i++)work[i]=0.;
Monty Montgomery's avatar
 
Monty Montgomery committed
232

Monty Montgomery's avatar
 
Monty Montgomery committed
233 234 235
    fftwf_execute(fftwf_weight);
    sliding_bark_average(freq,blocksize*2,width);
    iterbound=blocksize*iteration;
Monty Montgomery's avatar
 
Monty Montgomery committed
236
    if(iterbound<10)iterbound=10;
Monty Montgomery's avatar
 
Monty Montgomery committed
237

Monty Montgomery's avatar
 
Monty Montgomery committed
238
    if(count)reconstruct(work,freq,flag,epsilon,iterbound);
Monty Montgomery's avatar
 
Monty Montgomery committed
239

Monty Montgomery's avatar
 
Monty Montgomery committed
240 241
    if(out)
      for(i=0;i<blocksize/2;i++)
Monty Montgomery's avatar
 
Monty Montgomery committed
242
	out[i]=lap[i]+work[i+blocksize/2]*window[i];
Monty Montgomery's avatar
 
Monty Montgomery committed
243 244
    
    for(i=blocksize/2,j=0;i<blocksize;i++)
Monty Montgomery's avatar
 
Monty Montgomery committed
245
      lap[j++]=work[i+blocksize/2]*window[i];
Monty Montgomery's avatar
 
Monty Montgomery committed
246

Monty Montgomery's avatar
 
Monty Montgomery committed
247
  }else{
Monty Montgomery's avatar
 
Monty Montgomery committed
248

Monty Montgomery's avatar
 
Monty Montgomery committed
249 250
    if(out)
      for(i=0;i<blocksize/2;i++)
Monty Montgomery's avatar
 
Monty Montgomery committed
251
	out[i]=work[i+blocksize/2];
Monty Montgomery's avatar
 
Monty Montgomery committed
252 253
  
    for(i=blocksize/2,j=0;i<blocksize;i++)
Monty Montgomery's avatar
 
Monty Montgomery committed
254
      lap[j++]=work[i+blocksize/2]*window[i]*window[i];
Monty Montgomery's avatar
 
Monty Montgomery committed
255
  }
Monty Montgomery's avatar
 
Monty Montgomery committed
256 257
  for(i=blocksize/2;i<input_size;i++)
    lap[i]=0.;
Monty Montgomery's avatar
 
Monty Montgomery committed
258 259 260 261 262
}

/* called only by playback thread */
time_linkage *declip_read(time_linkage *in){
  int i;
Monty Montgomery's avatar
 
Monty Montgomery committed
263
  float local_trigger[input_ch];
Monty Montgomery's avatar
 
Monty Montgomery committed
264 265
  int total=0;
  int count[input_ch];
Monty Montgomery's avatar
 
Monty Montgomery committed
266
  float peak[input_ch];
Monty Montgomery's avatar
 
Monty Montgomery committed
267

Monty Montgomery's avatar
 
Monty Montgomery committed
268 269
  time_linkage dummy;

Monty Montgomery's avatar
 
Monty Montgomery committed
270 271
  float local_convergence;
  float local_iterations;
Monty Montgomery's avatar
 
Monty Montgomery committed
272 273 274 275 276 277 278 279
  
  pthread_mutex_lock(&master_mutex);
  local_convergence=convergence;
  local_iterations=iterations;
  memcpy(local_trigger,chtrigger,sizeof(local_trigger));
  pthread_mutex_unlock(&master_mutex);

  memset(count,0,sizeof(count));
Monty Montgomery's avatar
 
Monty Montgomery committed
280
  memset(peak,0,sizeof(peak));
Monty Montgomery's avatar
 
Monty Montgomery committed
281 282 283 284 285

  if(pending_blocksize!=blocksize){
    if(blocksize){
      free(widthlookup);
      free(window);
Monty Montgomery's avatar
 
Monty Montgomery committed
286 287 288
      fftwf_destroy_plan(fftwf_weight);
      fftwf_free(freq);
      fftwf_free(work);
Monty Montgomery's avatar
 
Monty Montgomery committed
289 290 291
    }
    blocksize=pending_blocksize;

Monty Montgomery's avatar
 
Monty Montgomery committed
292 293 294 295 296 297 298
    freq=fftwf_malloc((blocksize*2+2)*sizeof(freq));
    work=fftwf_malloc((blocksize*2)*sizeof(freq));
    fftwf_weight=fftwf_plan_dft_r2c_1d(blocksize*2,
				       work,
				       (fftwf_complex *)freq,
				       FFTW_MEASURE);

Monty Montgomery's avatar
 
Monty Montgomery committed
299 300
    lopad=1-rint(fromBark(toBark(0.)-width)*blocksize*2/input_rate);
    hipad=rint(fromBark(toBark(input_rate*.5)+width)*blocksize*2/input_rate)+lopad;
Monty Montgomery's avatar
 
Monty Montgomery committed
301
    widthlookup=malloc((hipad+1)*sizeof(*widthlookup));
Monty Montgomery's avatar
 
Monty Montgomery committed
302
    for(i=0;i<blocksize;i++){
Monty Montgomery's avatar
 
Monty Montgomery committed
303
      float bark=toBark(input_rate*i/(blocksize*2));
Monty Montgomery's avatar
 
Monty Montgomery committed
304 305
      int hi=rint(fromBark(bark-width)*(blocksize*2)/input_rate)-1+lopad;
      int lo=rint(fromBark(bark+width)*(blocksize*2)/input_rate)+1+lopad;
Monty Montgomery's avatar
 
Monty Montgomery committed
306 307 308 309
      widthlookup[i]=(hi<<16)+lo;
    }
    
    window=malloc(blocksize*sizeof(*window));
Monty Montgomery's avatar
 
Monty Montgomery committed
310
    for(i=0;i<blocksize;i++) window[i]=sin( M_PIl*i/blocksize );
Monty Montgomery's avatar
 
Monty Montgomery committed
311
    for(i=0;i<blocksize;i++) window[i]*=window[i];
Monty Montgomery's avatar
 
Monty Montgomery committed
312
    for(i=0;i<blocksize;i++) window[i]=sin(window[i]*M_PIl*.5);
Monty Montgomery's avatar
 
Monty Montgomery committed
313

Monty Montgomery's avatar
 
Monty Montgomery committed
314
    reconstruct_reinit(blocksize*2);
Monty Montgomery's avatar
 
Monty Montgomery committed
315 316
  }

Monty Montgomery's avatar
 
Monty Montgomery committed
317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335
  switch(fillstate){
  case 0: /* prime the lapping and cache */
    for(i=0;i<input_ch;i++){
      float *temp=in->data[i];
      total=0;
      memset(work+blocksize/2,0,sizeof(*work)*blocksize/2);
      memcpy(work+blocksize,temp,sizeof(*work)*blocksize/2);
      declip(lap[i],0,blocksize,
	     local_trigger[i],local_convergence,local_iterations,
	     &total,count+i,peak+i);
      
      memset(cache[i],0,sizeof(**cache)*input_size);
      in->data[i]=cache[i];
      cache[i]=temp;
    }
    cache_samples=in->samples;
    fillstate=1;
    out.samples=0;
    if(in->samples==in->size)goto tidy_up;
Monty Montgomery's avatar
 
Monty Montgomery committed
336
    
Monty Montgomery's avatar
 
Monty Montgomery committed
337 338 339 340 341 342 343 344 345 346 347 348
    for(i=0;i<input_ch;i++)
      memset(in->data[i],0,sizeof(**in->data)*in->size);
    in->samples=0;
    /* fall through */
  case 1: /* nominal processing */
    for(i=0;i<input_ch;i++){
      float *temp=cache[i];
      int j;
      total=0;
      for(j=0;j+blocksize<=out.size;j+=blocksize/2){
	memcpy(work+blocksize/2,temp+j,sizeof(*work)*blocksize);
	declip(lap[i],out.data[i]+j,blocksize,
Monty Montgomery's avatar
 
Monty Montgomery committed
349
	       local_trigger[i],local_convergence,local_iterations,
Monty Montgomery's avatar
 
Monty Montgomery committed
350
	       &total,count+i,peak+i);
Monty Montgomery's avatar
 
Monty Montgomery committed
351
      }
Monty Montgomery's avatar
 
Monty Montgomery committed
352 353 354 355 356 357 358 359 360
      memcpy(work+blocksize/2,temp+j,sizeof(*work)*blocksize/2);
      memcpy(work+blocksize,in->data[i],sizeof(*work)*blocksize/2);
      
      declip(lap[i],out.data[i]+j,blocksize,
	     local_trigger[i],local_convergence,local_iterations,
	     &total,count+i,peak+i);
      
      cache[i]=in->data[i];
      in->data[i]=temp;
Monty Montgomery's avatar
 
Monty Montgomery committed
361
    }
Monty Montgomery's avatar
 
Monty Montgomery committed
362 363 364 365 366 367
    out.samples=cache_samples;
    cache_samples=in->samples;
    if(out.samples<out.size)fillstate=2;
    break;
  case 2: /* we've pushed out EOF already */
    out.samples=0;
Monty Montgomery's avatar
 
Monty Montgomery committed
368
  }
Monty Montgomery's avatar
 
Monty Montgomery committed
369

Monty Montgomery's avatar
 
Monty Montgomery committed
370
  push_declip_feedback(count,peak,total);
Monty Montgomery's avatar
 
Monty Montgomery committed
371

Monty Montgomery's avatar
 
Monty Montgomery committed
372 373 374 375 376 377 378 379
 tidy_up:
  {
    int tozero=out.size-out.samples;
    if(tozero)
      for(i=0;i<out.channels;i++)
	memset(out.data[i]+out.samples,0,sizeof(**out.data)*tozero);
  }

Monty Montgomery's avatar
 
Monty Montgomery committed
380 381
  return &out;
}