5 * Copyright (C) 2002-2005 Monty and Xiph.Org
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)
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.
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.
26 #include <sys/types.h>
32 extern int input_rate
;
33 extern int input_size
;
36 typedef struct freq_feedback
{
37 feedback_generic parent_class
;
43 static feedback_generic
*new_freq_feedback(void){
44 freq_feedback
*ret
=calloc(1,sizeof(*ret
));
45 return (feedback_generic
*)ret
;
48 /* total, peak, rms are pulled in array[freqs][input_ch] order */
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
;
57 feedback_old(&ff
->feedpool
,(feedback_generic
*)f
);
61 for(i
=0;i
<ff
->fc
->bands
;i
++)
62 memcpy(peak
[i
],f
->peak
[i
],sizeof(**peak
)*ch
);
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
);
71 int freq_class_load(freq_class_setup
*f
,const float *frequencies
, int bands
){
73 memset(f
,0,sizeof(*f
));
75 f
->qblocksize
=input_size
;
78 /* fill in time window */
79 f
->window
=window_get(3,f
->qblocksize
);
81 f
->fftwf_buffer
= fftwf_malloc(sizeof(*f
->fftwf_buffer
) *
84 f
->fftwf_forward
=fftwf_plan_dft_r2c_1d(f
->qblocksize
*4,
86 (fftwf_complex
*)(f
->fftwf_buffer
),
89 f
->fftwf_backward
=fftwf_plan_dft_c2r_1d(f
->qblocksize
*4,
90 (fftwf_complex
*)(f
->fftwf_buffer
),
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 */
99 f
->ho_area
=calloc(bands
,sizeof(*f
->ho_area
));
100 f
->ho_window
=malloc(bands
*sizeof(*f
->ho_window
));
102 f
->ho_window
[i
]=calloc((f
->qblocksize
*2+1),sizeof(**f
->ho_window
));
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);
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];
114 if(localf
>=lastf
&& localf
<nextf
){
117 if(i
!=0)localwin
= sin((localf
-lastf
)/(thisf
-lastf
)*M_PIl
*.5);
119 if(i
+1!=bands
)localwin
= sin((nextf
-localf
)/(nextf
-thisf
)*M_PIl
*.5);
123 f
->ho_window
[i
][localbin
]+=localwin
*(1./32);
128 for(;j
<f
->qblocksize
*2+1;j
++){
129 float localf
= .5*j
*input_rate
/(f
->qblocksize
<<1);
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];
136 if(localf
>=lastf
&& localf
<nextf
){
139 if(i
!=0)localwin
= sin((localf
-lastf
)/(thisf
-lastf
)*M_PIl
*.5);
141 if(i
+1!=bands
)localwin
= sin((nextf
-localf
)/(nextf
-thisf
)*M_PIl
*.5);
144 f
->ho_window
[i
][j
]+=localwin
*localwin
;
149 for(i
=0;i
<bands
;i
++){
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
];
158 fftwf_execute(f
->fftwf_backward
);
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);
167 for(;j
<f
->qblocksize
*3;j
++)
168 f
->fftwf_buffer
[j
]=0.;
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);
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;
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
]);
186 f
->ho_area
[i
]=1./f
->ho_area
[i
];
192 /* called only by initial setup */
193 int freq_load(freq_state
*f
,freq_class_setup
*fc
,int ch
){
195 memset(f
,0,sizeof(*f
));
201 f
->cache1
=malloc(ch
*sizeof(*f
->cache1
));
202 f
->cache0
=malloc(ch
*sizeof(*f
->cache0
));
204 f
->cache1
[i
]=calloc(input_size
,sizeof(**f
->cache1
));
205 f
->cache0
[i
]=calloc(input_size
,sizeof(**f
->cache0
));
208 f
->activeP
=malloc(ch
*sizeof(*f
->activeP
));
209 f
->active1
=malloc(ch
*sizeof(*f
->active1
));
210 f
->active0
=malloc(ch
*sizeof(*f
->active0
));
213 f
->lap1
=malloc(ch
*sizeof(*f
->lap1
));
214 f
->lap0
=malloc(ch
*sizeof(*f
->lap0
));
215 f
->lapC
=malloc(ch
*sizeof(*f
->lapC
));
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
));
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
));
230 f
->out
.data
=malloc(ch
*sizeof(*f
->out
.data
));
232 f
->out
.data
[i
]=malloc(input_size
*sizeof(**f
->out
.data
));
237 /* called only in playback thread */
238 int freq_reset(freq_state
*f
){
239 /* reset cached pipe state */
241 while(pull_freq_feedback(f
,NULL
,NULL
));
245 static void freq_metric_work(float *x
,freq_class_setup
*c
,
246 float **peak
,float **rms
,int channel
){
248 float sq_mags
[c
->qblocksize
*2+1];
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.;
253 for(i
=0;i
<c
->bands
;i
++){
254 float *ho_window
=c
->ho_window
[i
];
257 for(j
=0;j
<c
->qblocksize
*2+1;j
++){
258 float val
=fabs(sq_mags
[j
]*ho_window
[j
]);
260 if(val
>lpeak
)lpeak
=val
;
262 rms
[i
][channel
]=todB(lrms
*.5*c
->ho_area
[i
])*.5;
263 peak
[i
][channel
]=todB(lpeak
)*.5;
267 static void fill_freq_buffer_helper(float *buffer
,float *window
,
268 float *cache
, float *in
,
269 int qblocksize
,int muted0
,int mutedC
,
271 /* place data in fftwf buffer */
272 memset(buffer
,0,sizeof(*buffer
)*qblocksize
);
275 memset(buffer
+qblocksize
,0,sizeof(*buffer
)*qblocksize
);
277 memcpy(buffer
+qblocksize
,cache
,sizeof(*buffer
)*qblocksize
);
280 memset(buffer
+qblocksize
*2,0,sizeof(*buffer
)*qblocksize
);
282 memcpy(buffer
+qblocksize
*2,in
,sizeof(*buffer
)*qblocksize
);
284 memset(buffer
+qblocksize
*3,0,sizeof(*buffer
)*qblocksize
);
286 /* window (if nonzero) */
287 if(!muted0
|| !mutedC
)
288 window_apply(buffer
+qblocksize
,window
,scale
,qblocksize
);
292 static void freq_work(freq_class_setup
*fc
,
298 void (*func
)(float *,int)){
300 int i
,j
,ch
=f
->out
.channels
;
302 u_int32_t outactive
=0;
304 f
->cache_samples
+=in
->samples
;
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
);
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
];
316 /* zero the feedback */
317 for(j
=0;j
<fc
->bands
;j
++){
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
);
328 /* pre- and post-extrapolate to avoid harsh edge features.
329 Account for muting in previous or current frame */
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
);
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
);
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
);
347 fill_freq_buffer_helper(fc
->fftwf_buffer
,
349 f
->cache0
[i
],in
->data
[i
],
350 fc
->qblocksize
,muted0
,mutedC
,
353 /* transform the time data */
354 fftwf_execute(fc
->fftwf_forward
);
356 if(activeC
)func(fc
->fftwf_buffer
,i
);
358 /* feedback and reverse transform */
360 freq_metric_work(fc
->fftwf_buffer
,fc
,f
->peak
,f
->rms
,i
);
364 fftwf_execute(fc
->fftwf_backward
);
370 /* push zeroed feedback */
372 } /* else bypass feedback */
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 */
382 float *temp
=out
->data
[i
];
383 out
->data
[i
]=f
->cache1
[i
];
386 outactive
|= (f
->mutemask1
& (1<<i
));
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
];
396 float *t1
=fc
->fftwf_buffer
;
397 float *t0
=t1
+input_size
;
398 float *tC
=t0
+input_size
;
399 float *tN
=tC
+input_size
;
404 /* lap the cache into the transform vector */
405 fill_freq_buffer_helper(fc
->fftwf_buffer
,
407 f
->cache0
[i
],in
->data
[i
],
408 fc
->qblocksize
,muted0
,mutedC
,1.);
411 if(!activeP
&& !active1
&& !active0
){
412 /* previously in a bypassed/inactive state; the lapping cache
413 will need to be re-prepared */
415 memset(l1
,0,sizeof(*l1
)*input_size
);
417 memcpy(l1
,c1
,sizeof(*l1
)*input_size
);
419 memset(l0
,0,sizeof(*l0
)*input_size
);
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
);
428 float *ox
=out
->data
[i
];
430 for(j
=0;j
<input_size
;j
++)
434 for(j
=0;j
<input_size
;j
++){
441 f
->activeP
[i
]=active1
;
442 f
->active1
[i
]=active0
;
443 f
->active0
[i
]=activeC
;
446 float *temp
=f
->cache1
[i
];
447 f
->cache1
[i
]=f
->cache0
[i
];
448 f
->cache0
[i
]=in
->data
[i
];
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. */
458 (freq_feedback
*)feedback_new(&f
->feedpool
,new_freq_feedback
);
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
));
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
));
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
));
476 feedback_push(&f
->feedpool
,(feedback_generic
*)ff
);
479 (freq_feedback
*)feedback_new(&f
->feedpool
,new_freq_feedback
);
481 feedback_push(&f
->feedpool
,(feedback_generic
*)ff
);
484 /* complete output linkage */
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
;
491 f
->mutemaskP
=f
->mutemask1
;
492 f
->mutemask1
=f
->mutemask0
;
493 f
->mutemask0
=in
->active
;
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
;
503 switch(f
->fillstate
){
504 case 0: /* prime the lapping and cache */
510 /* zero out lapping and cache state */
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
];
522 f
->mutemask0
=f
->mutemask1
=f
->mutemaskP
=in
->active
;
524 freq_work(fc
,f
,in
,0,visible
,active
,func
);
528 if(in
->samples
==input_size
)goto tidy_up
;
531 memset(in
->data
[i
],0,sizeof(**in
->data
)*input_size
);
535 case 1: /* finish priming the lapping and cache */
537 freq_work(fc
,f
,in
,0,visible
,active
,func
);
541 if(in
->samples
==input_size
)goto tidy_up
;
544 memset(in
->data
[i
],0,sizeof(**in
->data
)*input_size
);
548 case 2: /* nominal processing */
550 freq_work(fc
,f
,in
,&f
->out
,visible
,active
,func
);
552 if(f
->out
.samples
<input_size
)f
->fillstate
=3;
554 case 3: /* we've pushed out EOF already */
560 int tozero
=input_size
-f
->out
.samples
;
562 for(i
=0;i
<f
->out
.channels
;i
++)
563 memset(f
->out
.data
[i
]+f
->out
.samples
,0,sizeof(**f
->out
.data
)*tozero
);