1 /*
2  * CDDL HEADER START
3  *
4  * The contents of this file are subject to the terms of the
5  * Common Development and Distribution License, Version 1.0 only
6  * (the "License").  You may not use this file except in compliance
7  * with the License.
8  *
9  * You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE
10  * or http://www.opensolaris.org/os/licensing.
11  * See the License for the specific language governing permissions
12  * and limitations under the License.
13  *
14  * When distributing Covered Code, include this CDDL HEADER in each
15  * file and include the License file at usr/src/OPENSOLARIS.LICENSE.
16  * If applicable, add the following below this CDDL HEADER, with the
17  * fields enclosed by brackets "[]" replaced with your own identifying
18  * information: Portions Copyright [yyyy] [name of copyright owner]
19  *
20  * CDDL HEADER END
21  */
22 /*
23  * Copyright (c) 1992-2001 by Sun Microsystems, Inc.
24  * All rights reserved.
25  */
26 
27 #pragma ident	"%Z%%M%	%I%	%E% SMI"
28 
29 /*
30  * This is a impementation of sampling rate conversion for low-passed signals.
31  *
32  * To convert the input signal of sampling rate of fi to another rate of fo,
33  * the least common multiple of fi and fo is derived first:
34  *	fm = L * fi = M * fo.
35  * Then the input signal is up-sampled to fm by inserting (L -1) zero valued
36  * samples after each input sample, low-pass filtered, and down-smapled by
37  * saving one sample for every M samples. The implementation takes advantages
38  * of the (L - 1) zero values in the filter input and the (M - 1) output
39  * samples to be disregarded.
40  *
41  * If L = 1 or M = 1, a simpler decimation or interpolation is used.
42  */
43 #include <memory.h>
44 #include <math.h>
45 
46 #include <Resample.h>
47 
48 extern "C" {
49 	char *bcopy(char *, char *, int);
50 	char *memmove(char *, char *, int);
51 }
52 
53 #define	BCOPY(src, dest, num) memmove(dest, src, num)
54 
55 // convolve(), short2double() and double2short() are defined in Fir.cc.
56 extern double convolve(double *, double *, int);
57 extern void short2double(double *, short *, int);
58 extern short double2short(double);
59 
60 // generate truncated ideal LPF
sinc_coef(int fold,int order,double * coef)61 static void sinc_coef(int	fold,	// sample rate change
62 	int	order,			// LP FIR filter order
63 	double  *coef)			// filter coefficients
64 {
65 	int	i;
66 	float   alpha;
67 	double  bandwidth = M_PI / fold; // digital bandwidth of pass band
68 
69 	int half = order >> 1;
70 	if (order & 1) {		// order is odd, center = half + 0.5
71 		float center = half + 0.5;
72 		for (i = 0; i <= half; i++) {
73 			alpha = center - i;
74 			coef[i] = sin(bandwidth * alpha) / (M_PI * alpha);
75 		}
76 	} else {	// order is even, center = half
77 		for (i = 0; i < half; i++) {
78 			alpha = half - i;
79 			coef[i] = sin(bandwidth * alpha) / (M_PI * alpha);
80 		}
81 		coef[i++] = bandwidth / M_PI;
82 	}
83 	for (; i <= order; i++)		// symmetric FIR
84 		coef[i] = coef[order - i];
85 }
86 
87 /*
88  * poly_conv() = coef[0] * data[length - 1] +
89  *		 coef[inc_coef] * data[length - 2] +
90  *		 ...
91  *		 coef[(length - 1) * inc_coef] * data[0] +
92  *
93  * convolution of coef[order + 1] and data[length] up-sampled by a factor
94  * of inc_coef. The terms of coef[1, 2, ... inc_coef - 1] multiplying 0
95  * are ignored.
96  */
97 // polyphase filter convolution
98 double
poly_conv(double * coef,int order,int inc_coef,double * data,int length)99 poly_conv(double	*coef,		// filter coef array
100 	    int		order,		// filter order
101 	    int		inc_coef,	// 1-to-L up-sample for data[length]
102 	    double	*data,		// data array
103 	    int		length)		// data array length
104 {
105 	if ((order < 0) || (inc_coef < 1) || (length < 1))
106 		return (0.0);
107 	else {
108 		double sum = 0.0;
109 		double *coef_end = coef + order;
110 		double *data_end = data + length;
111 		while ((coef <= coef_end) && (data < data_end)) {
112 			sum += *coef * *--data_end;
113 			coef += inc_coef;
114 		}
115 		return (sum);
116 	}
117 }
118 
119 int
gcf(int a,int b)120 gcf(int a, int b)		// greatest common factor between a and b
121 {
122 	int remainder = a % b;
123 	return (remainder == 0)? b : gcf(b, remainder);
124 }
125 
126 void ResampleFilter::
updateState(double * in,int size)127 updateState(
128 	double *in,
129 	int size)
130 {
131 	if (up <= 1)
132 		Fir::updateState(in, size);
133 	else if (size >= num_state)
134 		memcpy(state, in + size - num_state,
135 		    num_state * sizeof (double));
136 	else {
137 		int old = num_state - size;
138 		BCOPY((char *)(state + size), (char *)state,
139 		    old * sizeof (double));
140 		memcpy(state + old, in, size * sizeof (double));
141 	}
142 }
143 
144 ResampleFilter::			// constructor
ResampleFilter(int rate_in,int rate_out)145 ResampleFilter(
146 	int rate_in,			// sampling rate of input signal
147 	int rate_out)			// sampling rate of output signal
148 {
149 	// filter sampling rate = common multiple of rate_in and rate_out
150 	int commonfactor = gcf(rate_in, rate_out);
151 	up = rate_out / commonfactor;
152 	down = rate_in / commonfactor;
153 
154 	int fold = (up > down)? up : down;	// take the bigger rate change
155 	order = (fold << 4) - 2;		// filter order = fold * 16 - 2
156 	coef = new double[order + 1];
157 	sinc_coef(fold, order, coef);		// required bandwidth = PI/fold
158 
159 	if (up > 1) {				// need (order/up) states
160 		num_state = (order + up  - 1) / up;
161 		state = new double[num_state];
162 		for (int i = 0; i < num_state; i++)	// reset states
163 			state[i] = 0.0;
164 	} else {
165 		num_state = order;
166 		state = new double[order];	// need order states
167 		resetState();
168 	}
169 	delay = (order + 1) >> 1;	// assuming symmetric FIR
170 	down_offset = 0;
171 	up_offset = 0;
172 }
173 
174 // down-to-1 decimation
175 int ResampleFilter::
decimate_noadjust(short * in,int size,short * out)176 decimate_noadjust(short	*in,
177 		int	size,
178 		short	*out)
179 {
180 	int	i;
181 
182 	if (size <= 0)
183 		return (0);
184 	else if (down <= 1)		// normal filter
185 		return (Fir::filter_noadjust(in, size, out));
186 	else if (size <= down_offset) {	// just update states
187 		update_short(in, size);
188 		down_offset -= size;
189 		return (0);
190 	}
191 
192 	double *in_buf = new double[size];
193 	short2double(in_buf, in, size);
194 
195 	// filter and decimate and output
196 	short   *out_ptr = out;
197 	int init_size = (size <= order)? size : order;
198 	for (i = down_offset; i < init_size; i += down)
199 		*out_ptr++ = double2short(convolve(coef, in_buf, i + 1) +
200 		    convolve(coef + i + 1, state + i, order - i));
201 	for (; i < size; i += down)
202 		*out_ptr++ = double2short(convolve(coef, in_buf + i - order,
203 		    order + 1));
204 	down_offset = i - size;
205 
206 	updateState(in_buf, size);
207 	delete in_buf;
208 	return (out_ptr - out);
209 }
210 
211 // decimate with group delay adjusted to 0
212 int ResampleFilter::
decimate(short * in,int size,short * out)213 decimate(short	*in,
214 	int	size,
215 	short	*out)
216 {
217 	if (delay <= 0)
218 		return (decimate_noadjust(in, size, out));
219 	else if (size <= delay) {
220 		update_short(in, size);
221 		delay -= size;
222 		return (0);
223 	} else {
224 		update_short(in, delay);
225 		in += delay;
226 		size -= delay;
227 		delay = 0;
228 		return (decimate_noadjust(in, size, out));
229 	}
230 }
231 
232 // flush decimate filter
233 int ResampleFilter::
decimate_flush(short * out)234 decimate_flush(short	*out)
235 {
236 	int num_in = Fir::getFlushSize();
237 	short *in = new short[num_in];
238 	memset(in, 0, num_in * sizeof (short));
239 	int num_out = decimate_noadjust(in, num_in, out);
240 	delay += num_in;
241 	delete in;
242 	return (num_out);
243 }
244 
245 // 1-to-up interpolation
246 int ResampleFilter::
interpolate_noadjust(short * in,int size,short * out)247 interpolate_noadjust(short	*in,
248 		    int		size,
249 		    short	*out)
250 {
251 	int	i, j;
252 
253 	if (size <= 0)
254 		return (0);
255 	else if (up <= 1)			// regular filter
256 		return (Fir::filter_noadjust(in, size, out));
257 
258 	double *in_buf = new double[size];
259 	short2double(in_buf, in, size);
260 	short *out_ptr = out;
261 	// befor the 1st input sample, generate  "-up_offset" output samples
262 	int coef_offset = up + up_offset;
263 	for (j = up_offset; j < 0; j++) {
264 		*out_ptr++ = double2short(up * poly_conv(coef + coef_offset,
265 		    order - coef_offset, up, state, num_state));
266 		coef_offset++;
267 	}
268 	// for each of the rest input samples, generate up output samples
269 	for (i = 1; i < size; i++) {
270 		for (j = 0; j < up; j++) {
271 			*out_ptr++ = double2short(up * (poly_conv(coef + j,
272 			    order - j, up, in_buf, i) + poly_conv(
273 			    coef + coef_offset, order - coef_offset, up, state,
274 			    num_state)));
275 			coef_offset++;
276 		}
277 	}
278 
279 	// for the last input samples, generate "up_offset + up" output samples
280 	for (j = 0; j < (up_offset + up); j++) {
281 		*out_ptr++ = double2short(up * (poly_conv(coef + j,
282 		    order - j, up, in_buf, size) + poly_conv(
283 		    coef + coef_offset, order - coef_offset, up, state,
284 		    num_state)));
285 		coef_offset++;
286 	}
287 	updateState(in_buf, size);
288 	delete in_buf;
289 	return (out_ptr - out);
290 }
291 
292 // flush interpolate filter
293 int ResampleFilter::
interpolate_flush(short * out)294 interpolate_flush(short	*out)
295 {
296 	int num = (Fir::getFlushSize() + up - 1) / up;
297 
298 	short *in = new short[num];
299 	memset(in, 0, num * sizeof (short));
300 	int out_num = interpolate_noadjust(in, num, out);
301 	delay += num * up;
302 	delete in;
303 	return (out_num);
304 }
305 
306 // interpolate with delay adjusted
307 int ResampleFilter::
interpolate(short * in,int size,short * out)308 interpolate(short *in,
309 	    int size,
310 	    short *out)
311 {
312 	if (size <= 0)
313 		return (interpolate_flush(out));
314 	else if (delay <= 0)
315 		return (interpolate_noadjust(in, size, out));
316 	else {
317 		int delay_in = (delay + up - 1) / up;
318 		if (size < delay_in)
319 			delay_in = size;
320 		double *in_buf = new double[delay_in];
321 		short2double(in_buf, in, delay_in);
322 		updateState(in_buf, delay_in);
323 		delete in_buf;
324 		delay -= delay_in * up;
325 		up_offset = delay;
326 		return (interpolate_noadjust(in + delay_in, size -
327 		    delay_in, out));
328 	}
329 }
330 
331 int ResampleFilter::
filter_noadjust(short * in,int size,short * out)332 filter_noadjust(short	*in,		// non-integer sampling rate conversion
333 	int	size,
334 	short	*out)
335 {
336 	int	i, j;
337 
338 	if (size <= 0)
339 		return (0);
340 	else if (up <= 1)
341 		return (decimate_noadjust(in, size, out));
342 	else if (down <= 1)
343 		return (interpolate_noadjust(in, size, out));
344 
345 	double *in_buf = new double[size];
346 	short2double(in_buf, in, size);
347 	short *init_out = out;
348 	int coef_offset = up_offset + down_offset + up;
349 
350 	/*
351 	 * before the 1st input sample,
352 	 * start from "up_offset + down_offset"th up-sampled sample
353 	 */
354 	for (j = up_offset + down_offset; j < 0; j += down) {
355 		*out++ = double2short(up * poly_conv(coef + coef_offset,
356 		    order - coef_offset, up, state, num_state));
357 		coef_offset += down;
358 	}
359 
360 	// process the input samples until the last one
361 	for (i = 1; i < size; i++) {
362 		for (; j < up; j += down) {
363 			*out++ = double2short(up * (poly_conv(coef + j,
364 			    order - j, up, in_buf, i) + poly_conv(
365 			    coef + coef_offset, order - coef_offset, up, state,
366 			    num_state)));
367 			coef_offset += down;
368 		}
369 		j -= up;
370 	}
371 
372 	// for the last input sample, end at the "up + up_offset"th
373 	for (; j < (up + up_offset); j += down) {
374 		*out++ = double2short(up * (poly_conv(coef + j, order - j, up,
375 		    in_buf, size) + poly_conv(coef + coef_offset,
376 		    order - coef_offset, up, state, num_state)));
377 		coef_offset += down;
378 	}
379 	down_offset = j - (up + up_offset);
380 
381 	updateState(in_buf, size);
382 	delete in_buf;
383 	return (out - init_out);
384 }
385 
386 int ResampleFilter::
getFlushSize(void)387 getFlushSize(void)
388 {
389 	int num_in = (Fir::getFlushSize() + up - 1) / up;
390 	return ((num_in * up + down - 1 - down_offset) / down);
391 }
392 
393 int ResampleFilter::
flush(short * out)394 flush(short	*out)		// flush resampling filter
395 
396 {
397 	if (down <= 1)
398 		return (interpolate_flush(out));
399 	else if (up <= 1)
400 		return (decimate_flush(out));
401 
402 	int num = (Fir::getFlushSize() + up - 1) / up;
403 
404 	short *in = new short[num];
405 	memset(in, 0, num * sizeof (short));
406 	int out_num = filter_noadjust(in, num, out);
407 	delete in;
408 	delay += num * up;
409 	return (out_num);
410 }
411 
412 /*
413  * sampling rate conversion with filter delay adjusted
414  */
415 int ResampleFilter::
filter(short * in,int size,short * out)416 filter(
417 	short	*in,
418 	int	size,
419 	short	*out)
420 {
421 	if (size <= 0)
422 		return (flush(out));
423 	else if (up <= 1)
424 		return (decimate(in, size, out));
425 	else if (down <= 1)
426 		return (interpolate(in, size, out));
427 	else if (delay <= 0)
428 		return (filter_noadjust(in, size, out));
429 	else {
430 		int delay_in = (delay + up - 1) / up;
431 		if (size < delay_in)
432 			delay_in = size;
433 		double *in_buf = new double[delay_in];
434 		short2double(in_buf, in, delay_in);
435 		updateState(in_buf, delay_in);
436 		delete in_buf;
437 		delay -= up * delay_in;
438 		if (delay <= 0) {
439 			up_offset = delay;
440 			down_offset = 0;
441 		}
442 		return (filter_noadjust(in + delay_in, size - delay_in, out));
443 	}
444 }
445