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