declip.c 16.4 KB
Newer Older
1 2 3 4
/*
 *
 *  postfish
 *    
Monty Montgomery's avatar
Monty Montgomery committed
5
 *      Copyright (C) 2002-2005 Monty and Xiph.Org
6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26
 *
 *  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>
27
#include <fftw3.h>
28
#include "reconstruct.h"
29
#include "window.h"
30
#include "feedback.h"
31 32 33 34 35 36

extern int input_rate;
extern int input_ch;
extern int input_size;

/* accessed only in playback thread/setup */
37

38
static fftwf_plan fftwf_weight;
39

40 41 42
static float *work;
static float *freq;

43 44 45
static int blocksize=0;
static int lopad=0,hipad=0;
static u_int32_t *widthlookup=0;
46 47 48 49 50

static float *leftwindow=0;
static float *rightwindow=0;
static int left=0;
static int right=0;
51
static float width=.5;
52
static float **lap;
53
static float **cache;
54
static u_int32_t cache_active;
55 56 57 58 59 60 61
static int cache_samples;
static int fillstate=0; /* 0: uninitialized
			   1: normal
			   2: eof processed */
static time_linkage out;

/* accessed across threads */
62
sig_atomic_t *declip_active;
63 64
int *declip_prev_active;

Monty Montgomery's avatar
Monty Montgomery committed
65
sig_atomic_t declip_visible=0;
66

67 68 69 70
sig_atomic_t *declip_chtrigger=0;
sig_atomic_t declip_pending_blocksize=0;
sig_atomic_t declip_convergence=0;
sig_atomic_t declip_iterations=0;
71 72 73 74

/* feedback! */
typedef struct declip_feedback{
  feedback_generic parent_class;
75
  float *peak;
76 77
  int   *clipcount;
  int   *total;
78 79 80 81 82 83 84
} 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));
85
  ret->peak=malloc((input_ch)*sizeof(*ret->peak));
86
  ret->total=malloc((input_ch)*sizeof(*ret->total));
87 88 89
  return (feedback_generic *)ret;
}

90
static void push_declip_feedback(int *clip,float *peak,int *total){
91
  int n=input_ch;
92 93 94
  declip_feedback *f=(declip_feedback *)
    feedback_new(&feedpool,new_declip_feedback);
  memcpy(f->clipcount,clip,n*sizeof(*clip));
95
  memcpy(f->peak,peak,n*sizeof(*peak));
96
  memcpy(f->total,total,n*sizeof(*total));
97 98 99
  feedback_push(&feedpool,(feedback_generic *)f);
}

100
int pull_declip_feedback(int *clip,float *peak,int *total){
101 102 103 104 105
  declip_feedback *f=(declip_feedback *)feedback_pull(&feedpool);

  if(!f)return 0;

  if(clip)memcpy(clip,f->clipcount,sizeof(*clip)*input_ch);
106
  if(peak)memcpy(peak,f->peak,sizeof(*peak)*input_ch);
107
  if(total)memcpy(total,f->total,sizeof(*total)*input_ch);
108 109 110 111

  feedback_old(&feedpool,(feedback_generic *)f);
  return 1;
}
112

113
static void apply_window(float *out,float *data,int sq){
114
  int i,j;
115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130
  int half=max(left,right);

  for(i=0;i<half-left;i++)
    out[i]=0;

  if(sq){
    for(j=0;i<half;i++,j++)
      out[i]=data[i]*leftwindow[j]*leftwindow[j];
    for(j=0;j<right;i++,j++)
      out[i]=data[i]*rightwindow[right-j]*rightwindow[right-j];
  }else{
    for(j=0;i<half;i++,j++)
      out[i]=data[i]*leftwindow[j];
    for(j=0;j<right;i++,j++)
      out[i]=data[i]*rightwindow[right-j];
  }
131

132 133 134
  for(;i<half*2;i++)
    out[i]=0;
}
135

136 137 138
static void setup_window(int lleft,int lright){
  left=lleft;
  right=lright;
139

140 141
  leftwindow=window_get(2,left);
  rightwindow=window_get(2,right);
142 143 144 145
}

static void setup_blocksize(int newblocksize){
  int i;
146

147 148
  if(blocksize)fftwf_destroy_plan(fftwf_weight);
  blocksize=newblocksize;
149 150 151 152
  fftwf_weight=fftwf_plan_dft_r2c_1d(blocksize*2,work,
					(fftwf_complex *)freq,
					FFTW_MEASURE);
  
153 154 155 156 157 158 159 160 161 162 163 164
  lopad=1-rint(fromBark(toBark(0.)-width)*blocksize*2/input_rate);
  hipad=rint(fromBark(toBark(input_rate*.5)+width)*blocksize*2/input_rate)+lopad;
  for(i=0;i<blocksize;i++){
    float bark=toBark(input_rate*i/(blocksize*2));
    int hi=rint(fromBark(bark-width)*(blocksize*2)/input_rate)-1+lopad;
    int lo=rint(fromBark(bark+width)*(blocksize*2)/input_rate)+1+lopad;
    widthlookup[i]=(hi<<16)+lo;
  }

  reconstruct_reinit(blocksize*2);
}

165 166
/* called only by initial setup */
int declip_load(void){
167
  int i,j;
168
  declip_active=calloc(input_ch,sizeof(*declip_active));
169
  declip_prev_active=calloc(input_ch,sizeof(*declip_prev_active));
170
  declip_chtrigger=malloc(input_ch*sizeof(*declip_chtrigger));
171
  for(i=0;i<input_ch;i++)
172
    declip_chtrigger[i]=10000;
173 174 175 176 177 178 179 180 181 182
  
  out.channels=input_ch;
  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));
183

184 185 186
  lap=malloc(input_ch*sizeof(*lap));
  for(i=0;i<input_ch;i++)
    lap[i]=malloc(input_size*sizeof(**lap));
187 188 189 190 191 192 193
  
  {    
    /* alloc for largest possible blocksize */
    int blocksize=input_size*2;
    int loestpad=1-rint(fromBark(toBark(0.)-width)*blocksize*2/input_rate);
    int hiestpad=rint(fromBark(toBark(input_rate*.5)+width)*blocksize*2/input_rate)+loestpad;
    widthlookup=malloc((hiestpad+1)*sizeof(*widthlookup));
194 195
    freq=fftwf_malloc((blocksize*2+2)*sizeof(*freq));
    work=fftwf_malloc((blocksize*2)*sizeof(*work));
196

197 198 199 200 201 202 203 204 205
    for(i=0,j=32;j<=blocksize*2;i++,j*=2){
      fftwf_weight=fftwf_plan_dft_r2c_1d(j,work,
					 (fftwf_complex *)freq,
					 FFTW_MEASURE);
      fftwf_destroy_plan(fftwf_weight);
    }
  }
  reconstruct_init(32,input_size*4);
  
206
  declip_pending_blocksize=input_size*2;
207 208 209 210 211 212 213
  return(0);
}

/* called only in playback thread */
int declip_reset(void){
  /* reset cached pipe state */
  fillstate=0;
214
  while(pull_declip_feedback(NULL,NULL,NULL));
215 216 217
  return 0;
}

218
int noisy=0;
219
static void sliding_bark_average(float *f,int n,float width){
220
  int i=0;
221 222
  double acc=0.,del=0.;
  double sec[hipad+1];
223 224 225

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

226
  for(i=0;i<n/2;i++){
227 228 229

    int hi=widthlookup[i]>>16;
    int lo=widthlookup[i]&(0xffff);
230
    float del=hypot(f[(i<<1)+1],f[i<<1])/(lo-hi);
231

232 233
    float hidel=del/((i-hi+lopad));
    float lodel=del/((lo-i-lopad));
234 235 236 237 238 239 240 241 242 243 244 245 246

    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;
  }

247 248
  for(i=0;i<n/2;i++){
    f[(i<<1)+1]=f[i<<1]=1./(acc*acc);
249 250 251
    del+=sec[i+lopad];
    acc+=del;
  }
252
  f[n+1]=f[n]=f[n-1];
253 254
}

255
/* work,freq are passed through the static buffer fftwf requires */
256
static void declip(int blocksize,float trigger,
257
		   float epsilon, float iteration,
258
		   int *runningtotal, int *runningcount){
259
  float flag[blocksize*2];
260
  int    iterbound,i,count=0;
261 262 263 264 265 266 267 268 269

  /* too many apps screw up proper output scaling, so previously
     clipped audio ends up getting rounded to just short of the rail
     upon export.  To avoid users accidentally shooting themselves in
     the foot, trigger clipping at -.05dB rather than 0dB even if 0dB
     is selected. This corresponds to mis-rounding 8 bit audio by 1.5
     steps.*/
  if(trigger>.99426)trigger=.99426; 

270
  for(i=blocksize/2;i<blocksize*3/2;i++){
271
    flag[i]=0.;
272
    if(work[i]>=trigger || work[i]<=-trigger){
273
      flag[i]=1.;
274 275 276
      count++;
    }
  }
277

278
  
279
  *runningtotal+=blocksize;
280
  *runningcount+=count;
281

282
  if(count){
283 284
    for(i=0;i<blocksize/2;i++)flag[i]=0.;
    for(i=blocksize*3/2;i<blocksize*2;i++)flag[i]=0.;
285
    apply_window(work+blocksize/2,work+blocksize/2,0);
286
    
287 288 289
    fftwf_execute(fftwf_weight);
    sliding_bark_average(freq,blocksize*2,width);
    iterbound=blocksize*iteration;
290
    if(iterbound<10)iterbound=10;
291
    
292
    reconstruct(work,freq,flag,epsilon,iterbound);
293

294
    apply_window(work+blocksize/2,work+blocksize/2,0);
295
  }else
296
    apply_window(work+blocksize/2,work+blocksize/2,1);
297

298 299 300 301
}

/* called only by playback thread */
time_linkage *declip_read(time_linkage *in){
302
  int i,j,k;
303
  float local_trigger[input_ch];
304
  int count[input_ch];
305
  int total[input_ch];
306
  float peak[input_ch];
307
  u_int32_t active=0;
308
  int next_blocksize=declip_pending_blocksize;
309
  int orig_blocksize;
310

311 312
  float local_convergence;
  float local_iterations;
313
  
314 315 316 317
  for(i=0;i<input_ch;i++)
    local_trigger[i]=declip_chtrigger[i]*.0001;
  local_iterations=declip_iterations*.0001;
  local_convergence=fromdB(declip_convergence*.1);
318 319

  memset(count,0,sizeof(count));
320
  memset(peak,0,sizeof(peak));
321
  memset(total,0,sizeof(total));
322

323 324
  switch(fillstate){
  case 0: /* prime the lapping and cache */
325

326 327 328 329
    /* set up for the blocksize we're actually using for now */
    {
      setup_blocksize(next_blocksize);
      setup_window(blocksize/2,blocksize/2);
330 331
    }

332
    for(i=0;i<input_ch;i++){
333 334 335 336
      int channel_active=declip_active[i]; 
      declip_prev_active[i]=channel_active;

      /* peak feedback */
337
      if(declip_visible && !mute_channel_muted(in->active,i)){
338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355
	float *l=in->data[i];
	for(j=0;j<in->samples;j++)
	  if(fabs(l[j])>peak[i])peak[i]=fabs(l[j]);
      }

      if(channel_active){
	
	/* fill work with the block spanning cache/in (first 1/4, last 1/4 are zeroed) */
	memset(work,0,sizeof(*work)*blocksize);
	memcpy(work+blocksize,in->data[i],sizeof(*work)*blocksize/2);
	memset(work+blocksize+blocksize/2,0,sizeof(*work)*blocksize/2);
	declip(blocksize,local_trigger[i],local_convergence,local_iterations,
	       total+i,count+i);

	/* second half of work goes to lap */
	memcpy(lap[i],work+blocksize,sizeof(*work)*blocksize/2);

	/* now iterate the pieces purely within in */
356
	for(j=0;j+blocksize<=input_size;j+=blocksize/2){
357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373
	  memset(work,0,sizeof(*work)*blocksize);
	  memcpy(work+blocksize/2,in->data[i]+j,sizeof(*work)*blocksize);
	  memset(work+blocksize+blocksize/2,0,sizeof(*work)*blocksize/2);

	  declip(blocksize,local_trigger[i],local_convergence,local_iterations,
		 total+i,count+i);

	  /* second half of work goes to lap */
	  {
	    float *llap=lap[i]+j;
	    float *lwork=work+blocksize/2;
	    for(k=0;k<blocksize/2;k++)
	      llap[k]+=lwork[k];
	    memcpy(llap+k,lwork+k,sizeof(*work)*blocksize/2);
	  }
	}

374
      }//else no declipping to do, so direct cache/lap buffer rotation */
375

376
      {
377 378 379 380 381
	float *temp=cache[i];
	cache[i]=in->data[i];
	in->data[i]=temp;
	memset(temp,0,sizeof(*temp)*input_size);
      }
382 383
    }
    cache_samples=in->samples;
384
    cache_active=in->active;
385 386
    fillstate=1;
    out.samples=0;
387
    if(in->samples==input_size)break;
388
    
389
    for(i=0;i<input_ch;i++)
390
      memset(in->data[i],0,sizeof(**in->data)*input_size);
391 392
    in->samples=0;
    /* fall through */
393

394
  case 1: /* nominal processing */
395 396 397 398 399 400 401 402 403 404 405 406
    orig_blocksize=blocksize;

    /* the 'gap' transition and finishing off the output block is done
       first as it may need to handle a blocksize transition (and a
       temporary transition window */

    if(next_blocksize != orig_blocksize){
      if(next_blocksize > orig_blocksize) setup_blocksize(next_blocksize);
      setup_window(orig_blocksize/2,next_blocksize/2);
    }

    /* the gap piece is also special in that it may need to deal with
407
       a transition to/from bypass */
408
    
409
    for(i=0;i<input_ch;i++){
410 411 412
      int channel_active=declip_active[i];

      /* peak feedback */
413
      if(declip_visible && !mute_channel_muted(in->active,i)){
414 415 416
	float *l=in->data[i];
	for(j=0;j<in->samples;j++)
	  if(fabs(l[j])>peak[i])peak[i]=fabs(l[j]);
417
      }
418 419


420 421
      if(mute_channel_muted(cache_active,i)){
	
422 423 424 425
	/* we may need cache for a later transition, so keep it up to date */
	float *temp=cache[i];
	cache[i]=in->data[i];
	in->data[i]=temp;
426 427 428 429 430
	
	/* zero the lap */
	if(!mute_channel_muted(in->active,i))
	  memset(lap[i],0,sizeof(*lap[i])*input_size);
	
431
      }else{
432 433 434 435 436 437 438
	
	active|=(1<<i); /* audible output in out.data[i] */
	
	if(mute_channel_muted(in->active,i)){
	  if(declip_prev_active[i]){
	    /* Cache: Muted=False, Bypass=False
	       Input: Muted=True,  Bypass=X     */
439
	    
440
	    /* transition to mute, so lap is finished output.  Rotate all */
441 442 443
	    float *temp=cache[i];
	    cache[i]=in->data[i];
	    in->data[i]=temp;
444 445 446 447
	    
	    temp=out.data[i];
	    out.data[i]=lap[i];
	    lap[i]=temp;
448
	  }else{
449 450 451 452 453 454
	    /* Cache: Muted=False, Bypass=True
	       Input: Muted=True,  Bypass=X     */
	    
	    /* rotate in/cache/out, transition out */
	    float *temp=out.data[i];
	    out.data[i]=cache[i];
455 456
	    cache[i]=in->data[i];
	    in->data[i]=temp;
457
	    
458 459
	  }
	}else{
460 461 462 463 464 465 466 467
	  if(!declip_prev_active[i]){
	    if(!channel_active){
	      /* Cache: Muted=False, Bypass=True
		 Input: Muted=False, Bypass=True     */
	      
	      /* all bypass! rotate in/cache/out */
	      float *temp=out.data[i];
	      out.data[i]=cache[i];
468 469 470 471
	      cache[i]=in->data[i];
	      in->data[i]=temp;
	    }else{
	      /* Cache: Muted=False, Bypass=True
472 473 474 475 476 477
		 Input: Muted=False, Bypass=False     */
	      
	      /* transition the lap; right window to left of in */
	      for(j=0;j<right;j++)
		lap[i][j]=in->data[i][j]*
		  rightwindow[right-j]*rightwindow[right-j];
478
	      
479
	      /* all rotate in/cache/out */
480 481 482 483 484 485
	      float *temp=out.data[i];
	      out.data[i]=cache[i];
	      cache[i]=in->data[i];
	      in->data[i]=temp;
	    }
	  }else{
486 487 488
	    if(!channel_active){
	      /* Cache: Muted=False, Bypass=False
		 Input: Muted=False, Bypass=True     */
489
	      
490 491 492 493 494 495 496 497 498 499 500
	      /* finish off lap, then rotate all */
	      /* left window to end of cache */
	      for(j=input_size-left,k=0;j<input_size;j++,k++)
		lap[i][j]+=cache[i][j]*leftwindow[k]*leftwindow[k];
	      float *temp=cache[i];
	      cache[i]=in->data[i];
	      in->data[i]=temp;
	      
	      temp=out.data[i];
	      out.data[i]=lap[i];
	      lap[i]=temp;
501
	    }else{
502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519
	      /* Cache: Muted=False, Bypass=False
		 Input: Muted=False, Bypass=False */
	      
	      /* nominal case; the only one involving declipping the gap */
	      memset(work,0,sizeof(*work)*blocksize/2);
	      memcpy(work+blocksize/2,cache[i]+input_size-blocksize/2,sizeof(*work)*blocksize/2);
	      memcpy(work+blocksize,in->data[i],sizeof(*work)*blocksize/2);
	      memset(work+blocksize+blocksize/2,0,sizeof(*work)*blocksize/2);
	      
	      declip(blocksize,local_trigger[i],local_convergence,local_iterations,
		     total+i,count+i);
	      
	      /* finish lap from last frame */
	      {
		float *llap=lap[i]+input_size-blocksize/2;
		float *lwork=work+blocksize/2;
		for(j=0;j<blocksize/2;j++)
		  llap[j]+=lwork[j];
520
	      }
521 522 523 524 525 526 527 528 529 530 531
	      /* rotate buffers */
	      float *temp=out.data[i];
	      out.data[i]=lap[i];
	      lap[i]=temp;
	      
	      temp=in->data[i];
	      in->data[i]=cache[i];
	      cache[i]=temp;
	      
	      /* begin lap for this frame */
	      memcpy(lap[i],work+blocksize,sizeof(*work)*blocksize/2);
532 533 534 535
	    }
	  }
	}
      }
536

537
      declip_prev_active[i]=channel_active;
538
    }
539
    
540
    /* also rotate metadata */
541 542
    out.samples=cache_samples;
    cache_samples=in->samples;
543
    cache_active=in->active;
544
    
545 546 547 548 549 550 551 552 553 554 555 556
    /* finish transition to new blocksize (if a change is in progress) */
    if(next_blocksize != orig_blocksize){
      if(next_blocksize <= orig_blocksize) setup_blocksize(next_blocksize);
      setup_window(blocksize/2,blocksize/2);
    }

    /* declip the rest of the current frame */
    for(i=0;i<input_ch;i++){
      if(!mute_channel_muted(cache_active,i)){
	/* declip */
	if(declip_prev_active[i]){
	  
557
	  for(j=0;j+blocksize<=input_size;j+=blocksize/2){
558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574
	    memset(work,0,sizeof(*work)*blocksize);
	    memcpy(work+blocksize/2,cache[i]+j,sizeof(*work)*blocksize);
	    memset(work+blocksize+blocksize/2,0,sizeof(*work)*blocksize/2);
	    declip(blocksize,local_trigger[i],local_convergence,local_iterations,
		   total+i,count+i);
	    
	    {
	      float *llap=lap[i]+j;
	      float *lwork=work+blocksize/2;
	      for(k=0;k<blocksize/2;k++)
		llap[k]+=lwork[k];
	      memcpy(llap+k,lwork+k,sizeof(*work)*blocksize/2);
	    }
	  }
	}
      }
    }
575
    if(out.samples<input_size)fillstate=2;
576 577 578
    break;
  case 2: /* we've pushed out EOF already */
    out.samples=0;
579
  }
580

581 582 583 584 585 586 587
  push_declip_feedback(count,peak,total); /* we can push one (and
                                             exactly one) for every
                                             block that comes in *or*
                                             one for every block that
                                             goes out.  In declip,
                                             it's for every block that
                                             comes in */
588

589
  {
590
    int tozero=input_size-out.samples;
591 592 593 594 595
    if(tozero)
      for(i=0;i<out.channels;i++)
	memset(out.data[i]+out.samples,0,sizeof(**out.data)*tozero);
  }

596
  out.active=active;
597 598
  return &out;
}