1 /* OpenCP Module Player
2  * copyright (c) 2011 Jindřich Makovička <makovick@gmail.com>
3  *
4  * C routines for FPU mixer
5  *
6  * This program is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software
18  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
19  */
20 
21 #include <assert.h>
22 #include <math.h>
23 
24 #define MAXVOICES MIXF_MAXCHAN
25 
26 #define state dwmixfa_state
27 dwmixfa_state_t state;
28 
29 #if 0
30 float   *tempbuf;               /* pointer to 32 bit mix buffer (nsamples * 4) */
31 void    *outbuf;                /* pointer to mixed buffer (nsamples * 2) */
32 uint32_t nsamples;              /* # of samples to mix */
33 uint32_t nvoices;               /* # of voices to mix */
34 uint32_t freqw[MAXVOICES];      /* frequency (whole part) */
35 uint32_t freqf[MAXVOICES];      /* frequency (fractional part) */
36 float   *smpposw[MAXVOICES];    /* sample position (whole part (pointer!)) */
37 uint32_t smpposf[MAXVOICES];    /* sample position (fractional part) */
38 float   *loopend[MAXVOICES];    /* pointer to loop end */
39 uint32_t looplen[MAXVOICES];    /* loop length in samples */
40 float    volleft[MAXVOICES];    /* float: left volume (1.0=normal) */
41 float    volright[MAXVOICES];   /* float: rite volume (1.0=normal) */
42 float    rampleft[MAXVOICES];   /* float: left volramp (dvol/sample) */
43 float    rampright[MAXVOICES];  /* float: rite volramp (dvol/sample) */
44 uint32_t voiceflags[MAXVOICES]; /* voice status flags */
45 float    ffreq[MAXVOICES];      /* filter frequency (0<=x<=1) */
46 float    freso[MAXVOICES];      /* filter resonance (0<=x<1) */
47 float    fadeleft=0.0;          /* 0 */
48 float    fl1[MAXVOICES];        /* filter lp buffer */
49 float    fb1[MAXVOICES];        /* filter bp buffer */
50 float    faderight=0.0;         /* 0 */
51 int      isstereo;              /* flag for stereo output */
52 int      outfmt;                /* output format */
53 float    voll=0.0;
54 float    volr=0.0;
55 float    ct0[256];              /* interpolation tab for s[-1] */
56 float    ct1[256];              /* interpolation tab for s[0] */
57 float    ct2[256];              /* interpolation tab for s[1] */
58 float    ct3[256];              /* interpolation tab for s[2] */
59 struct mixfpostprocregstruct *postprocs;
60                                 /* pointer to postproc list */
61 uint32_t samprate;              /* sampling rate */
62 
63 static float volrl;
64 static float volrr;
65 #endif
66 
67 static const float cremoveconst = 0.992;
68 static const float minampl = 0.0001;
69 
70 #if 0
71 static uint32_t mixlooplen; /* 32bit in assembler used, decimal. lenght of loop in samples*/
72 static uint32_t looptype; /* 32bit in assembler used, local version of voiceflags[N] */
73 static float ffrq;
74 static float frez;
75 static float __fl1;
76 static float __fb1;
77 #endif
78 
79 typedef void(*clippercall)(float *input, void *output, uint_fast32_t count);
80 
81 static void clip_16s(float *input, void *output, uint_fast32_t count);
82 static void clip_16u(float *input, void *output, uint_fast32_t count);
83 static void clip_8s(float *input, void *output, uint_fast32_t count);
84 static void clip_8u(float *input, void *output, uint_fast32_t count);
85 
86 static const clippercall clippers[4] = {clip_8s, clip_8u, clip_16s, clip_16u};
87 
88 typedef void(*mixercall)(float *destptr, float **sample_pos, uint32_t *sample_pos_fract, uint32_t sample_pitch, uint32_t sample_pitch_fract, float *loopend);
89 
90 void
prepare_mixer(void)91 prepare_mixer (void)
92 {
93 	int i;
94 
95 	state.fadeleft  = 0.0;
96 	state.faderight = 0.0;
97 	state.volrl = 0.0;
98 	state.volrr = 0.0;
99 
100 	for (i = 0; i < MAXVOICES; i++)
101 		state.volleft[i] = dwmixfa_state.volright[i] = 0.0;
102 }
103 
104 static inline
clearbufm(float * samples,int count)105 void clearbufm(float *samples, int count)
106 {
107 	int i;
108 
109 	for (i = 0; i < count; i++)
110 	{
111 		*samples++ = state.fadeleft;
112 		state.fadeleft *= cremoveconst;
113 	}
114 }
115 
116 static inline
clearbufs(float * samples,int count)117 void clearbufs(float *samples, int count)
118 {
119 	int i;
120 
121 	for (i = 0; i < count; i++)
122 	{
123 		*samples++ = state.fadeleft;
124 		*samples++ = state.faderight;
125 		state.fadeleft *= cremoveconst;
126 		state.faderight *= cremoveconst;
127 	}
128 }
129 
130 
131 static void
mix_0(float * destptr,float ** sample_pos,uint32_t * sample_pos_fract,uint32_t sample_pitch,uint32_t sample_pitch_fract,float * loopend)132 mix_0(float *destptr,
133       float **sample_pos, uint32_t *sample_pos_fract,
134       uint32_t sample_pitch, uint32_t sample_pitch_fract,
135       float *loopend)
136 {
137 	int i;
138 
139 	for (i = 0; i < state.nsamples; i++)
140 	{
141 		*sample_pos_fract += sample_pitch_fract;
142 		*sample_pos += sample_pitch + (*sample_pos_fract >> 16);
143 		*sample_pos_fract &= 0xffff;
144 		while (*sample_pos >= loopend)
145 		{
146 			if (!(state.looptype & MIXF_LOOPED))
147 			{
148 				state.looptype &= ~MIXF_PLAYING;
149 				goto out;
150 			}
151 			assert(state.mixlooplen > 0);
152 			*sample_pos -= state.mixlooplen;
153 		}
154 	}
155 out:
156 	;
157 }
158 
159 static inline float
filter_none(float sample)160 filter_none(float sample)
161 {
162 	return sample;
163 }
164 
165 static inline float
filter_mixf(float sample)166 filter_mixf(float sample)
167 {
168 	state.__fb1  =  state.__fb1  *  state.frez  +  state.ffrq  *  (  sample  -  state.__fl1  );
169 
170 	return state.__fl1  +=  state.__fb1;
171 }
172 
173 static inline float
interp_none(float * samples,uint_fast16_t sample_pos_fract)174 interp_none(float* samples, uint_fast16_t sample_pos_fract)
175 {
176 	return *samples;
177 }
178 
179 static inline float
interp_lin(float * samples,uint_fast16_t sample_pos_fract)180 interp_lin(float* samples, uint_fast16_t sample_pos_fract)
181 {
182 	return samples[0]
183 	        + (float)sample_pos_fract / 65536.0
184 	        * (samples[1] - samples[0]);
185 }
186 
187 static inline float
interp_cub(float * samples,uint_fast16_t sample_pos_fract)188 interp_cub(float* samples, uint_fast16_t sample_pos_fract)
189 {
190 	int idx = sample_pos_fract >> 8;
191 	return samples[0] * state.ct0[idx]
192 	        + samples[1] * state.ct1[idx]
193 	        + samples[2] * state.ct2[idx]
194 	        + samples[3] * state.ct3[idx];
195 }
196 
197 #define MIX_TEMPLATE(NAME, STEREO, INTERP, FILTER)                      \
198 static void                                                             \
199 mix##NAME(float *destptr,                                               \
200        float **sample_pos, uint32_t *sample_pos_fract,                  \
201        uint32_t sample_pitch, uint32_t sample_pitch_fract,              \
202        float *loopend)                                                  \
203 {                                                                       \
204     int i = 0;                                                          \
205     float sample;                                                       \
206                                                                         \
207     for (i = 0; i < state.nsamples; i++)                                \
208       {                                                                 \
209         sample = filter_##FILTER(interp_##INTERP(*sample_pos, *sample_pos_fract)); \
210         *destptr++ += state.voll * sample;                              \
211         state.voll += state.volrl;                                      \
212         if (STEREO) {                                                   \
213             *destptr++ += state.volr * sample;                          \
214             state.volr += state.volrr;                                  \
215         }                                                               \
216                                                                         \
217         *sample_pos_fract += sample_pitch_fract;                        \
218         *sample_pos += sample_pitch + (*sample_pos_fract >> 16);        \
219         *sample_pos_fract &= 0xffff;                                    \
220                                                                         \
221         while (*sample_pos >= loopend)                                  \
222           {                                                             \
223             if (!(state.looptype & MIXF_LOOPED)) {                      \
224                 state.looptype &= ~MIXF_PLAYING;                        \
225                 goto fade;                                              \
226             }                                                           \
227             assert(state.mixlooplen > 0);                               \
228             *sample_pos -= state.mixlooplen;                            \
229           }                                                             \
230       }                                                                 \
231     return;                                                             \
232                                                                         \
233 fade:                                                                   \
234                                                                         \
235     for (; i < state.nsamples; i++)                                     \
236       {                                                                 \
237         *destptr++ += state.voll * sample;                              \
238         state.voll += state.volrl;                                      \
239         if (STEREO) {                                                   \
240             *destptr++ += state.volr * sample;                          \
241             state.volr += state.volrr;                                  \
242         }                                                               \
243     }                                                                   \
244                                                                         \
245     state.fadeleft += state.voll * sample;                              \
246     if (STEREO) {                                                       \
247         state.faderight += state.volr * sample;                         \
248     }                                                                   \
249 }
250 
251 MIX_TEMPLATE(m_n, 0, none, none)
252 MIX_TEMPLATE(s_n, 1, none, none)
253 MIX_TEMPLATE(m_i, 0, lin, none)
254 MIX_TEMPLATE(s_i, 1, lin, none)
255 MIX_TEMPLATE(m_i2, 0, cub, none)
256 MIX_TEMPLATE(s_i2, 1, cub, none)
257 MIX_TEMPLATE(m_nf, 0, none, mixf)
258 MIX_TEMPLATE(s_nf, 1, none, mixf)
259 MIX_TEMPLATE(m_if, 0, lin, mixf)
260 MIX_TEMPLATE(s_if, 1, lin, mixf)
261 MIX_TEMPLATE(m_i2f, 0, cub, mixf)
262 MIX_TEMPLATE(s_i2f, 1, cub, mixf)
263 
264 static const mixercall mixers[16] = {
265 	mixm_n,   mixs_n,   mixm_i,  mixs_i,
266 	mixm_i2,  mixs_i2,  mix_0,   mix_0,
267 	mixm_nf,  mixs_nf,  mixm_if, mixs_if,
268 	mixm_i2f, mixs_i2f, mix_0,   mix_0
269 };
270 
271 void
mixer(void)272 mixer (void)
273 {
274 	int voice;
275 	struct mixfpostprocregstruct *pp;
276 
277 	if (fabsf(state.fadeleft) < minampl)
278 		state.fadeleft = 0.0;
279 
280 	if (fabsf(state.faderight) < minampl)
281 		state.faderight = 0.0;
282 
283 	if (state.nsamples == 0)
284 		return;
285 
286 	if (state.isstereo)
287 		clearbufs(state.tempbuf, state.nsamples);
288 	else
289 		clearbufm(state.tempbuf, state.nsamples);
290 
291 	for (voice = state.nvoices - 1; voice >= 0; voice--)
292 	{
293 		mixercall mixer;
294 
295 		if (!(state.voiceflags[voice] & MIXF_PLAYING))
296 			continue;
297 
298 		state.looptype = state.voiceflags[voice];
299 		state.voll = state.volleft[voice];
300 		state.volr = state.volright[voice];
301 		state.volrl = state.rampleft[voice];
302 		state.volrr = state.rampright[voice];
303 
304 		state.ffrq = state.ffreq[voice];
305 		state.frez = state.freso[voice];
306 		state.__fl1 = state.fl1[voice];
307 		state.__fb1 = state.fb1[voice];
308 
309 		state.mixlooplen = state.looplen[voice];
310 
311 /*
312 		assert((state.freqf[voice] & 0xffff) == 0);
313 		assert((state.smpposf[voice] & 0xffff) == 0);
314 */
315 		mixer = mixers[(state.isstereo | state.voiceflags[voice]) & 0xf];
316 		state.smpposf[voice] >>= 16;
317 		mixer(state.tempbuf,
318 		      &state.smpposw[voice], &state.smpposf[voice],
319 		      state.freqw[voice], state.freqf[voice] >> 16,
320 		      state.loopend[voice]);
321 		state.smpposf[voice] <<= 16;
322 
323 		state.voiceflags[voice] = state.looptype;
324 		state.volleft[voice] = state.voll;
325 		state.volright[voice] = state.volr;
326 		state.fl1[voice] = state.__fl1;
327 		state.fb1[voice] = state.__fb1;
328 	}
329 
330 	for (pp = state.postprocs; pp; pp = pp->next)
331 		pp->Process(state.tempbuf, state.nsamples, state.samprate, state.isstereo);
332 
333 	clippers[state.outfmt](state.tempbuf, state.outbuf, state.isstereo ? 2 * state.nsamples : state.nsamples);
334 }
335 
336 static void
clip_16s(float * input,void * output,uint_fast32_t count)337 clip_16s(float *input, void *output, uint_fast32_t count)
338 {
339 	int16_t *out = output;
340 	int i;
341 
342 	for (i = 0; i < count; i++, input++, out++)
343 	{
344 		int_fast32_t s = *input;
345 		if (s > 32767)
346 			*out = 32767;
347 		else if (s < -32768)
348 			*out = -32768;
349 		else
350 			*out = s;
351 	}
352 }
353 
clip_16u(float * input,void * output,uint_fast32_t count)354 static void clip_16u(float *input, void *output, uint_fast32_t count)
355 {
356 	uint16_t *out = output;
357 	int i;
358 
359 	for (i = 0; i < count; i++, input++, out++)
360 	{
361 		int_fast32_t s = *input;
362 		if (s > 32767)
363 			*out = 65535;
364 		else if (s < -32768)
365 			*out = 0;
366 		else
367 			*out = s + 32768;
368 	}
369 }
370 
clip_8s(float * input,void * output,uint_fast32_t count)371 static void clip_8s(float *input, void *output, uint_fast32_t count)
372 {
373 	int8_t *out = output;
374 	int i;
375 
376 	for (i = 0; i < count; i++, input++, out++)
377 	{
378 		int s = (int)(*input) >> 8;
379 		if (s > 127)
380 			*out = 127;
381 		else if (s < -128)
382 			*out = -128;
383 		else
384 			*out = s;
385 	}
386 }
387 
clip_8u(float * input,void * output,uint_fast32_t count)388 static void clip_8u(float *input, void *output, uint_fast32_t count)
389 {
390 	uint8_t *out = output;
391 	int i;
392 
393 	for (i = 0; i < count; i++, input++, out++)
394 	{
395 		int s = *input;
396 		if (s > 127)
397 			*out = 255;
398 		else if (s < -128)
399 			*out = 0;
400 		else
401 			*out = s + 128;
402 	}
403 }
404 
405 void
getchanvol(int n,int len)406 getchanvol(int n, int len)
407 {
408 	float *sample_pos = state.smpposw[n];
409 	int sample_pos_fract = state.smpposf[n] >> 16;
410 	float sum = 0.0;
411 	int i;
412 
413 	if (state.voiceflags[n] & MIXF_PLAYING)
414 	{
415 		for (i = 0; i < state.nsamples; i++)
416 		{
417 			sum += fabsf(*sample_pos);
418 
419 			sample_pos_fract += state.freqf[n] >> 16;
420 			sample_pos += state.freqw[n] + (sample_pos_fract >> 16);
421 			sample_pos_fract &= 0xffff;
422 			while (sample_pos >= state.loopend[n])
423 			{
424 				if (!(state.voiceflags[n] & MIXF_LOOPED))
425 				{
426 					state.voiceflags[n] &= ~MIXF_PLAYING;
427 					goto out;
428 				}
429 				assert(state.looplen[n] > 0);
430 				sample_pos -= state.looplen[n];
431 			}
432 		}
433 	}
434 
435 out:
436 
437 	sum /= state.nsamples;
438 	state.voll = sum * state.volleft[n];
439 	state.volr = sum * state.volright[n];
440 }
441