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 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 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 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:: 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 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:: 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:: 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:: 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:: 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:: 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:: 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:: 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:: 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:: 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:: 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