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