postfish/freq.c

   1 /*
   2  *
   3  *  postfish
   4  *
   5  *      Copyright (C) 2002-2005 Monty and Xiph.Org
   6  *
   7  *  Postfish is free software; you can redistribute it and/or modify
   8  *  it under the terms of the GNU General Public License as published by
   9  *  the Free Software Foundation; either version 2, or (at your option)
  10  *  any later version.
  11  *
  12  *  Postfish is distributed in the hope that it will be useful,
  13  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  *  GNU General Public License for more details.
  16  *
  17  *  You should have received a copy of the GNU General Public License
  18  *  along with Postfish; see the file COPYING.  If not, write to the
  19  *  Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
  20  *
  21  *
  22  */
  23
  24 #include "postfish.h"
  25 #include <math.h>
  26 #include <sys/types.h>
  27 #include "feedback.h"
  28 #include "freq.h"
  29 #include "lpc.h"
  30 #include "window.h"
  31
  32 extern int input_rate;
  33 extern int input_size;
  34
  35 /* feedback! */
  36 typedef struct freq_feedback{
  37   feedback_generic parent_class;
  38   float **peak;
  39   float **rms;
  40   int bypass;
  41 } freq_feedback;
  42
  43 static feedback_generic *new_freq_feedback(void){
  44   freq_feedback *ret=calloc(1,sizeof(*ret));
  45   return (feedback_generic *)ret;
  46 }
  47
  48 /* total, peak, rms are pulled in array[freqs][input_ch] order */
  49
  50 int pull_freq_feedback(freq_state *ff,float **peak,float **rms){
  51   freq_feedback *f=(freq_feedback *)feedback_pull(&ff->feedpool);
  52   int i,ch=ff->out.channels;
  53
  54   if(!f)return 0;
  55
  56   if(f->bypass){
  57     feedback_old(&ff->feedpool,(feedback_generic *)f);
  58     return 2;
  59   }else{
  60     if(peak)
  61       for(i=0;i<ff->fc->bands;i++)
  62         memcpy(peak[i],f->peak[i],sizeof(**peak)*ch);
  63     if(rms)
  64       for(i=0;i<ff->fc->bands;i++)
  65         memcpy(rms[i],f->rms[i],sizeof(**rms)*ch);
  66     feedback_old(&ff->feedpool,(feedback_generic *)f);
  67     return 1;
  68   }
  69 }
  70
  71 int freq_class_load(freq_class_setup *f,const float *frequencies, int bands){
  72   int i,j;
  73   memset(f,0,sizeof(*f));
  74
  75   f->qblocksize=input_size;
  76   f->bands=bands;
  77
  78   /* fill in time window */
  79   f->window=window_get(3,f->qblocksize);
  80
  81   f->fftwf_buffer = fftwf_malloc(sizeof(*f->fftwf_buffer) *
  82                                  (f->qblocksize*4+2));
  83
  84   f->fftwf_forward=fftwf_plan_dft_r2c_1d(f->qblocksize*4,
  85                                          f->fftwf_buffer,
  86                                          (fftwf_complex *)(f->fftwf_buffer),
  87                                          FFTW_MEASURE);
  88
  89   f->fftwf_backward=fftwf_plan_dft_c2r_1d(f->qblocksize*4,
  90                                           (fftwf_complex *)(f->fftwf_buffer),
  91                                           f->fftwf_buffer,
  92                                           FFTW_MEASURE);
  93
  94   /* unlike old postfish, we offer all frequencies via smoothly
  95      supersampling the spectrum */
  96   /* I'm too lazy to figure out the integral symbolically, use this
  97      fancy CPU thingy for something */
  98
  99   f->ho_area=calloc(bands,sizeof(*f->ho_area));
 100   f->ho_window=malloc(bands*sizeof(*f->ho_window));
 101   for(i=0;i<bands;i++)
 102     f->ho_window[i]=calloc((f->qblocksize*2+1),sizeof(**f->ho_window));
 103
 104   /* first, build the first-pass desired, supersampled response */
 105   for(j=0;j<(((f->qblocksize*2+1)/10)<<5);j++){
 106     float localf= .5*j*input_rate/(f->qblocksize<<6);
 107     int localbin= j>>5;
 108
 109     for(i=0;i<bands;i++){
 110       float lastf=(i>0?frequencies[i-1]:0);
 111       float thisf=frequencies[i];
 112       float nextf=frequencies[i+1];
 113
 114       if(localf>=lastf && localf<nextf){
 115         float localwin=1.;
 116         if(localf<thisf){
 117           if(i!=0)localwin= sin((localf-lastf)/(thisf-lastf)*M_PIl*.5);
 118         }else{
 119           if(i+1!=bands)localwin= sin((nextf-localf)/(nextf-thisf)*M_PIl*.5);
 120         }
 121
 122         localwin*=localwin;
 123         f->ho_window[i][localbin]+=localwin*(1./32);
 124       }
 125     }
 126   }
 127   j>>=5;
 128   for(;j<f->qblocksize*2+1;j++){
 129     float localf= .5*j*input_rate/(f->qblocksize<<1);
 130
 131     for(i=0;i<bands;i++){
 132       float lastf=(i>0?frequencies[i-1]:0);
 133       float thisf=frequencies[i];
 134       float nextf=frequencies[i+1];
 135
 136       if(localf>=lastf && localf<nextf){
 137         float localwin=1.;
 138         if(localf<thisf){
 139           if(i!=0)localwin= sin((localf-lastf)/(thisf-lastf)*M_PIl*.5);
 140         }else{
 141           if(i+1!=bands)localwin= sin((nextf-localf)/(nextf-thisf)*M_PIl*.5);
 142         }
 143
 144         f->ho_window[i][j]+=localwin*localwin;
 145       }
 146     }
 147   }
 148
 149   for(i=0;i<bands;i++){
 150
 151     /* window each desired response in the time domain so that our
 152        convolution is properly padded against being circular */
 153     memset(f->fftwf_buffer,0,sizeof(*f->fftwf_buffer)*
 154            (f->qblocksize*4+2));
 155     for(j=0;j<f->qblocksize*2;j++)
 156       f->fftwf_buffer[j*2]=f->ho_window[i][j];
 157
 158     fftwf_execute(f->fftwf_backward);
 159
 160     /* window response in time */
 161     for(j=0;j<f->qblocksize;j++){
 162       float val=cos(j*M_PI/(f->qblocksize*2));
 163       val=sin(val*val*M_PIl*.5);
 164       f->fftwf_buffer[j]*= sin(val*val*M_PIl*.5);
 165     }
 166
 167     for(;j<f->qblocksize*3;j++)
 168       f->fftwf_buffer[j]=0.;
 169
 170     for(;j<f->qblocksize*4;j++){
 171       float val=sin((j-f->qblocksize*3)*M_PI/(f->qblocksize*2));
 172       val=sin(val*val*M_PIl*.5);
 173       f->fftwf_buffer[j]*=sin(val*val*M_PIl*.5);
 174     }
 175
 176     /* back to frequency; this is all-real data still */
 177     fftwf_execute(f->fftwf_forward);
 178     for(j=0;j<f->qblocksize*4+2;j++)
 179       f->fftwf_buffer[j]/=f->qblocksize*4;
 180
 181     /* now take what we learned and distill it a bit */
 182     for(j=0;j<f->qblocksize*2+1;j++){
 183       f->ho_window[i][j]=f->fftwf_buffer[j*2];
 184       f->ho_area[i]+=fabs(f->ho_window[i][j]);
 185     }
 186     f->ho_area[i]=1./f->ho_area[i];
 187   }
 188
 189   return 0;
 190 }
 191
 192 /* called only by initial setup */
 193 int freq_load(freq_state *f,freq_class_setup *fc,int ch){
 194   int i;
 195   memset(f,0,sizeof(*f));
 196
 197   f->fc=fc;
 198
 199   f->fillstate=0;
 200   f->cache_samples=0;
 201   f->cache1=malloc(ch*sizeof(*f->cache1));
 202   f->cache0=malloc(ch*sizeof(*f->cache0));
 203   for(i=0;i<ch;i++){
 204     f->cache1[i]=calloc(input_size,sizeof(**f->cache1));
 205     f->cache0[i]=calloc(input_size,sizeof(**f->cache0));
 206   }
 207
 208   f->activeP=malloc(ch*sizeof(*f->activeP));
 209   f->active1=malloc(ch*sizeof(*f->active1));
 210   f->active0=malloc(ch*sizeof(*f->active0));
 211
 212
 213   f->lap1=malloc(ch*sizeof(*f->lap1));
 214   f->lap0=malloc(ch*sizeof(*f->lap0));
 215   f->lapC=malloc(ch*sizeof(*f->lapC));
 216   for(i=0;i<ch;i++){
 217     f->lap1[i]=calloc(fc->qblocksize,sizeof(**f->lap1));
 218     f->lap0[i]=calloc(fc->qblocksize,sizeof(**f->lap0));
 219     f->lapC[i]=calloc(fc->qblocksize,sizeof(**f->lapC));
 220   }
 221
 222   f->peak=malloc(fc->bands*sizeof(*f->peak));
 223   f->rms=malloc(fc->bands*sizeof(*f->rms));
 224   for(i=0;i<fc->bands;i++){
 225     f->peak[i]=malloc(ch*sizeof(**f->peak));
 226     f->rms[i]=malloc(ch*sizeof(**f->rms));
 227   }
 228
 229   f->out.channels=ch;
 230   f->out.data=malloc(ch*sizeof(*f->out.data));
 231   for(i=0;i<ch;i++)
 232     f->out.data[i]=malloc(input_size*sizeof(**f->out.data));
 233
 234   return(0);
 235 }
 236
 237 /* called only in playback thread */
 238 int freq_reset(freq_state *f){
 239   /* reset cached pipe state */
 240   f->fillstate=0;
 241   while(pull_freq_feedback(f,NULL,NULL));
 242   return 0;
 243 }
 244
 245 static void freq_metric_work(float *x,freq_class_setup *c,
 246                              float **peak,float **rms,int channel){
 247   int i,j;
 248   float sq_mags[c->qblocksize*2+1];
 249
 250   for(i=0;i<c->qblocksize*2+1;i++)
 251     sq_mags[i]=(x[i*2]*x[i*2]+x[i*2+1]*x[i*2+1])*64.;
 252
 253   for(i=0;i<c->bands;i++){
 254     float *ho_window=c->ho_window[i];
 255     float lrms=0.;
 256     float lpeak=0.;
 257     for(j=0;j<c->qblocksize*2+1;j++){
 258       float val=fabs(sq_mags[j]*ho_window[j]);
 259       lrms+=val;
 260       if(val>lpeak)lpeak=val;
 261     }
 262     rms[i][channel]=todB(lrms*.5*c->ho_area[i])*.5;
 263     peak[i][channel]=todB(lpeak)*.5;
 264   }
 265 }
 266
 267 static void fill_freq_buffer_helper(float *buffer,float *window,
 268                                     float *cache, float *in,
 269                                     int qblocksize,int muted0,int mutedC,
 270                                     float scale){
 271   /* place data in fftwf buffer */
 272   memset(buffer,0,sizeof(*buffer)*qblocksize);
 273
 274   if(muted0)
 275     memset(buffer+qblocksize,0,sizeof(*buffer)*qblocksize);
 276   else
 277     memcpy(buffer+qblocksize,cache,sizeof(*buffer)*qblocksize);
 278
 279   if(mutedC)
 280     memset(buffer+qblocksize*2,0,sizeof(*buffer)*qblocksize);
 281   else
 282     memcpy(buffer+qblocksize*2,in,sizeof(*buffer)*qblocksize);
 283
 284   memset(buffer+qblocksize*3,0,sizeof(*buffer)*qblocksize);
 285
 286   /* window (if nonzero) */
 287   if(!muted0 || !mutedC)
 288     window_apply(buffer+qblocksize,window,scale,qblocksize);
 289
 290 }
 291
 292 static void freq_work(freq_class_setup *fc,
 293                       freq_state *f,
 294                       time_linkage *in,
 295                       time_linkage *out,
 296                       int      *visible,
 297                       int      *active,
 298                       void (*func)(float *,int)){
 299
 300   int i,j,ch=f->out.channels;
 301   int have_feedback=0;
 302   u_int32_t outactive=0;
 303
 304   f->cache_samples+=in->samples;
 305
 306   for(i=0;i<ch;i++){
 307     int mutedC=mute_channel_muted(in->active,i);
 308     int muted0=mute_channel_muted(f->mutemask0,i);
 309     int muted1=mute_channel_muted(f->mutemask1,i);
 310
 311     int activeC=active[i] && !(muted0 && mutedC);
 312     int active0=f->active0[i];
 313     int active1=f->active1[i];
 314     int activeP=f->activeP[i];
 315
 316     /* zero the feedback */
 317     for(j=0;j<fc->bands;j++){
 318       f->peak[j][i]=-150.;
 319       f->rms[j][i]=-150.;
 320     }
 321
 322     /* processing pathway depends on active|visible.  If we're visible & !active, the transform
 323        data is thrown away without being manipulated or used in output (it is merely measured) */
 324     int trans_active= (visible[i] || active[i]) && !(muted0 && mutedC);
 325
 326     if(trans_active){
 327
 328       /* pre- and post-extrapolate to avoid harsh edge features.
 329          Account for muting in previous or current frame */
 330       if(f->fillstate==0){
 331         if(mutedC)memset(in->data[i],0,sizeof(**in->data)*input_size);
 332         if(muted0)memset(f->cache0[i],0,sizeof(**f->cache0)*input_size);
 333         preextrapolate_helper(in->data[i],input_size,
 334                               f->cache0[i],input_size);
 335       }
 336
 337       if(in->samples<input_size){
 338         if(mutedC)memset(in->data[i],0,sizeof(**in->data)*input_size);
 339         if(muted0)memset(f->cache0[i],0,sizeof(**f->cache0)*input_size);
 340
 341         postextrapolate_helper(f->cache0[i],input_size,
 342                                in->data[i],in->samples,
 343                                in->data[i]+in->samples,
 344                                input_size-in->samples);
 345       }
 346
 347       fill_freq_buffer_helper(fc->fftwf_buffer,
 348                               fc->window,
 349                               f->cache0[i],in->data[i],
 350                               fc->qblocksize,muted0,mutedC,
 351                               .25/fc->qblocksize);
 352
 353       /* transform the time data */
 354       fftwf_execute(fc->fftwf_forward);
 355
 356       if(activeC)func(fc->fftwf_buffer,i);
 357
 358       /* feedback and reverse transform */
 359       if(visible[i]){
 360         freq_metric_work(fc->fftwf_buffer,fc,f->peak,f->rms,i);
 361         have_feedback=1;
 362       }
 363       if(activeC)
 364         fftwf_execute(fc->fftwf_backward);
 365       else
 366         trans_active=0;
 367
 368     }else{
 369       if(visible[i]){
 370         /* push zeroed feedback */
 371         have_feedback=1;
 372       } /* else bypass feedback */
 373     }
 374
 375     /* output data pathway depends on activity over the past four
 376        frames (including this one); draw from transform (if any) and
 377        lap, cache and lap, or just cache? */
 378     if(!activeP && !active1 && !active0 && !activeC){
 379       /* bypass; rotate the cache */
 380
 381       if(out){
 382         float *temp=out->data[i];
 383         out->data[i]=f->cache1[i];
 384         f->cache1[i]=temp;
 385
 386         outactive|= (f->mutemask1 & (1<<i));
 387       }
 388     }else{
 389       float *w2=fc->window;
 390       float *l0=f->lap0[i];
 391       float *l1=f->lap1[i];
 392       float *lC=f->lapC[i];
 393       float *c0=f->cache0[i];
 394       float *c1=f->cache1[i];
 395
 396       float *t1=fc->fftwf_buffer;
 397       float *t0=t1+input_size;
 398       float *tC=t0+input_size;
 399       float *tN=tC+input_size;
 400
 401       outactive|= (1<<i);
 402
 403       if(!trans_active){
 404         /* lap the cache into the transform vector */
 405         fill_freq_buffer_helper(fc->fftwf_buffer,
 406                                 fc->window,
 407                                 f->cache0[i],in->data[i],
 408                                 fc->qblocksize,muted0,mutedC,1.);
 409       }
 410
 411       if(!activeP && !active1 && !active0){
 412         /* previously in a bypassed/inactive state; the lapping cache
 413            will need to be re-prepared */
 414         if(muted1)
 415           memset(l1,0,sizeof(*l1)*input_size);
 416         else
 417           memcpy(l1,c1,sizeof(*l1)*input_size);
 418         if(muted0)
 419           memset(l0,0,sizeof(*l0)*input_size);
 420         else
 421           for(j=0;j<input_size;j++)
 422             l0[j]= c0[j]*w2[input_size-j];
 423         memset(lC,0,sizeof(*lC)*input_size);
 424
 425       }
 426
 427       if(out){
 428         float *ox=out->data[i];
 429
 430         for(j=0;j<input_size;j++)
 431           ox[j]= l1[j]+t1[j];
 432       }
 433
 434       for(j=0;j<input_size;j++){
 435         l1[j]= l0[j]+t0[j];
 436         l0[j]= lC[j]+tC[j];
 437         lC[j]= tN[j];
 438       }
 439     }
 440
 441     f->activeP[i]=active1;
 442     f->active1[i]=active0;
 443     f->active0[i]=activeC;
 444
 445     {
 446       float *temp=f->cache1[i];
 447       f->cache1[i]=f->cache0[i];
 448       f->cache0[i]=in->data[i];
 449       in->data[i]=temp;
 450     }
 451   }
 452
 453   /* log feedback metrics; this logs one for every call, rather than
 454    every output; that's fine; nothing flushes until first output, and
 455    any playback halt will kill off stragglers.  Sync is unaffected. */
 456   if(have_feedback){
 457     freq_feedback *ff=
 458       (freq_feedback *)feedback_new(&f->feedpool,new_freq_feedback);
 459
 460     if(!ff->peak){
 461       ff->peak=calloc(fc->bands,sizeof(*ff->peak));
 462       for(i=0;i<fc->bands;i++)
 463         ff->peak[i]=malloc(ch*sizeof(**ff->peak));
 464     }
 465     if(!ff->rms){
 466       ff->rms=calloc(fc->bands,sizeof(*ff->rms));
 467       for(i=0;i<fc->bands;i++)
 468         ff->rms[i]=malloc(ch*sizeof(**ff->rms));
 469     }
 470
 471     for(i=0;i<fc->bands;i++){
 472       memcpy(ff->peak[i],f->peak[i],ch*sizeof(**f->peak));
 473       memcpy(ff->rms[i],f->rms[i],ch*sizeof(**f->rms));
 474     }
 475     ff->bypass=0;
 476     feedback_push(&f->feedpool,(feedback_generic *)ff);
 477   }else{
 478     freq_feedback *ff=
 479       (freq_feedback *)feedback_new(&f->feedpool,new_freq_feedback);
 480     ff->bypass=1;
 481     feedback_push(&f->feedpool,(feedback_generic *)ff);
 482   }
 483
 484   /* complete output linkage */
 485   if(out){
 486     out->active=outactive;
 487     f->out.samples=(f->cache_samples>input_size?input_size:f->cache_samples);
 488     f->cache_samples-=f->out.samples;
 489   }
 490
 491   f->mutemaskP=f->mutemask1;
 492   f->mutemask1=f->mutemask0;
 493   f->mutemask0=in->active;
 494 }
 495
 496 /* called only by playback thread */
 497 time_linkage *freq_read(time_linkage *in, freq_state *f,
 498                         int *visible, int *active,
 499                         void (*func)(float *,int i)){
 500   int i,ch=f->out.channels;
 501   freq_class_setup *fc=f->fc;
 502
 503   switch(f->fillstate){
 504   case 0: /* prime the lapping and cache */
 505     if(in->samples==0){
 506       f->out.samples=0;
 507       return &f->out;
 508     }
 509
 510     /* zero out lapping and cache state */
 511     for(i=0;i<ch;i++){
 512       memset(f->lap1[i],0,sizeof(**f->lap1)*fc->qblocksize);
 513       memset(f->lap0[i],0,sizeof(**f->lap0)*fc->qblocksize);
 514       memset(f->cache0[i],0,sizeof(**f->cache0)*input_size);
 515       memset(f->cache1[i],0,sizeof(**f->cache1)*input_size);
 516       f->activeP[i]=active[i];
 517       f->active1[i]=active[i];
 518       f->active0[i]=active[i];
 519     }
 520
 521     f->cache_samples=0;
 522     f->mutemask0=f->mutemask1=f->mutemaskP=in->active;
 523
 524     freq_work(fc,f,in,0,visible,active,func);
 525
 526     f->fillstate=1;
 527     f->out.samples=0;
 528     if(in->samples==input_size)goto tidy_up;
 529
 530     for(i=0;i<ch;i++)
 531       memset(in->data[i],0,sizeof(**in->data)*input_size);
 532     in->samples=0;
 533     /* fall through */
 534
 535   case 1: /* finish priming the lapping and cache */
 536
 537     freq_work(fc,f,in,0,visible,active,func);
 538
 539     f->fillstate=2;
 540     f->out.samples=0;
 541     if(in->samples==input_size)goto tidy_up;
 542
 543     for(i=0;i<ch;i++)
 544       memset(in->data[i],0,sizeof(**in->data)*input_size);
 545     in->samples=0;
 546     /* fall through */
 547
 548   case 2: /* nominal processing */
 549
 550     freq_work(fc,f,in,&f->out,visible,active,func);
 551
 552     if(f->out.samples<input_size)f->fillstate=3;
 553     break;
 554   case 3: /* we've pushed out EOF already */
 555     f->out.samples=0;
 556   }
 557
 558  tidy_up:
 559   {
 560     int tozero=input_size-f->out.samples;
 561     if(tozero)
 562       for(i=0;i<f->out.channels;i++)
 563         memset(f->out.data[i]+f->out.samples,0,sizeof(**f->out.data)*tozero);
 564   }
 565
 566   return &f->out;
 567 }