00001
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046 #include "dsp.h"
00047
00048
00049 #define LOOP_UNROLL 6
00050 #define LOOP_UNROLL_PLUS_ONE 7
00051
00052
00053
00054 #define NB_PTS_TO_INTERPOLATE 3
00055
00056 #define DSP16_RESAMPLING_FUNCTION_NAME(x_num, data) \
00057 TPASTE2(dsp16_resampling_kernel_x, x_num),
00058
00059 #define DSP16_RESAMPLING_NO_LOOP_FUNCTION_NAME(x_num, data) \
00060 TPASTE2(dsp16_resampling_no_loop_kernel_x, x_num),
00061
00062 #define DSP16_RESAMPLING_FILTER(x_num, data) \
00063 sum += ph[(x_num+data)] * pvect2[-(x_num+data)];
00064
00065 static void dsp16_resampling_polynomial_interpolation(dsp16_t y[4], dsp16_t output_y[NB_PTS_TO_INTERPOLATE]);
00066
00067 static int dsp_op_gcd(int m, int n)
00068 {
00069 int r;
00070 if (m < n)
00071 {
00072 int temp;
00073 temp = m;
00074 m = n;
00075 n = temp;
00076 }
00077 r = m - (m / n) * n;
00078 while(r != 0)
00079 {
00080 m = n;
00081 n = r;
00082 r = m - (m / n) * n;
00083 }
00084 return n;
00085 }
00086
00087 static void resampling_windowing(dsp16_t *vect1, int size)
00088 {
00089 dsp16_win_hann(vect1, size);
00090 }
00091
00092 dsp_resampling_t *dsp16_resampling_setup(int input_sample_rate, int output_sample_rate, int buffer_size, int filter_order, int nb_channels, malloc_fct_t malloc_fct, dsp_resampling_options_t options)
00093 {
00094 dsp_resampling_t *dsp_resampling;
00095 int interpolation_factor, divisor, i;
00096 size_t memory_to_allocate_for_last_samples, memory_to_allocate_for_fir_coefs, memory_to_allocate_for_context;
00097
00098
00099 divisor = dsp_op_gcd(input_sample_rate, output_sample_rate);
00100 interpolation_factor = output_sample_rate / divisor;
00101
00102
00103 buffer_size += filter_order;
00104
00105
00106
00107 if (filter_order >= buffer_size && buffer_size > Max(2, NB_PTS_TO_INTERPOLATE))
00108 return NULL;
00109 if ((input_sample_rate * interpolation_factor) / output_sample_rate < 2 && input_sample_rate != output_sample_rate)
00110 return NULL;
00111
00112
00113 memory_to_allocate_for_last_samples = sizeof(dsp16_t) * buffer_size * nb_channels + 4;
00114 memory_to_allocate_for_fir_coefs = sizeof(dsp16_t) * filter_order * interpolation_factor + 4;
00115 memory_to_allocate_for_context = sizeof(dsp_resampling_context_t) * nb_channels + 4;
00116 if (!(dsp_resampling = (dsp_resampling_t *) malloc_fct(memory_to_allocate_for_last_samples
00117 + memory_to_allocate_for_fir_coefs
00118 + memory_to_allocate_for_context
00119 + sizeof(dsp_resampling_t))))
00120 return NULL;
00121
00122
00123
00124 dsp_resampling->last_samples = (void *) &dsp_resampling[1];
00125 dsp_resampling->last_samples = (void *) Align_up((int) dsp_resampling->last_samples, 4);
00126
00127 dsp_resampling->fir_coefs = (void *) ((int) dsp_resampling->last_samples + memory_to_allocate_for_last_samples);
00128 dsp_resampling->fir_coefs = (void *) Align_up((int) dsp_resampling->fir_coefs, 4);
00129
00130 dsp_resampling->context = (dsp_resampling_context_t *) ((int) dsp_resampling->fir_coefs + memory_to_allocate_for_fir_coefs);
00131 dsp_resampling->context = (dsp_resampling_context_t *) Align_up((int) dsp_resampling->context, 4);
00132
00133
00134 dsp_resampling->fs = input_sample_rate;
00135 dsp_resampling->interpolation_factor = interpolation_factor;
00136 dsp_resampling->decimation_factor = input_sample_rate / divisor;
00137 dsp_resampling->buffer_size = buffer_size;
00138 dsp_resampling->nb_channels = nb_channels;
00139 dsp_resampling->filter_order = filter_order;
00140 dsp_resampling->link_required = false;
00141 for(i=0; i<nb_channels; i++)
00142 {
00143 dsp_resampling->context[i].input_buffer_pos = 0;
00144 dsp_resampling->context[i].index = 0;
00145 }
00146
00147
00148 if (options == DSP_RESAMPLING_OPTIONS_NORMALIZE_FILTER_COEFFICIENTS)
00149 {
00150 S32 sum_max;
00151 dsp16_t *p;
00152
00153 dsp16_filt_lpfirdesign(
00154 dsp_resampling->fir_coefs,
00155 output_sample_rate / 2,
00156 input_sample_rate * dsp_resampling->interpolation_factor,
00157 dsp_resampling->filter_order * dsp_resampling->interpolation_factor,
00158 resampling_windowing,
00159 DSP_FILT_DESIGN_OPTIONS_NORMALIZE);
00160
00161
00162 dsp16_filt_interpolation_coefsort(
00163 dsp_resampling->fir_coefs,
00164 filter_order,
00165 dsp_resampling->interpolation_factor);
00166
00167
00168 p = (dsp16_t *) dsp_resampling->fir_coefs;
00169 sum_max = (S32) DSP16_Q(1.) - 1;
00170 for(i=0; i<dsp_resampling->filter_order * dsp_resampling->interpolation_factor; i+=filter_order)
00171 {
00172 S32 sum = 0;
00173 int j;
00174 for(j=0; j<filter_order; j++)
00175 sum += (S32) *p++;
00176 if (sum_max < sum)
00177 sum_max = sum;
00178 }
00179 sum_max = (DSP16_Q(1.) << DSP16_QB) / (sum_max + 1);
00180 dsp16_vect_realmul(dsp_resampling->fir_coefs, dsp_resampling->fir_coefs, dsp_resampling->filter_order * dsp_resampling->interpolation_factor, sum_max);
00181 }
00182 else
00183 {
00184 dsp16_filt_lpfirdesign(
00185 dsp_resampling->fir_coefs,
00186 output_sample_rate / 2,
00187 input_sample_rate * dsp_resampling->interpolation_factor,
00188 dsp_resampling->filter_order * dsp_resampling->interpolation_factor,
00189 resampling_windowing,
00190 DSP_FILT_DESIGN_OPTIONS_NONE);
00191
00192
00193 dsp16_filt_interpolation_coefsort(
00194 dsp_resampling->fir_coefs,
00195 filter_order,
00196 dsp_resampling->interpolation_factor);
00197 }
00198
00199
00200 dsp16_vect_zeropad(dsp_resampling->last_samples, buffer_size * nb_channels, buffer_size * nb_channels);
00201
00202 return dsp_resampling;
00203 }
00204
00205 #if LOOP_UNROLL > 4
00206
00207 #define DSP16_RESAMPLING_KERNEL_X_FCT(x_num, data) \
00208 static int TPASTE2(dsp16_resampling_kernel_x, x_num)(dsp16_t *vect1, dsp16_t *vect2, int vect2_size, dsp16_t *h, int n_tap, int interpolation_ratio, int decimation_ratio, int *pcounter, int *pn) \
00209 { \
00210 S32 sum = 0; \
00211 int i, k, n; \
00212 dsp16_t *ph; \
00213 dsp16_t *pvect1; \
00214 dsp16_t *pvect2; \
00215 int size = 0; \
00216 int counter; \
00217 \
00218 pvect1 = vect1; \
00219 \
00220 n = *pn; \
00221 counter = *pcounter; \
00222 vect2_size -= n_tap; \
00223 while(n < vect2_size) \
00224 { \
00225 for(k=counter; k<interpolation_ratio; k += decimation_ratio) \
00226 { \
00227 sum = 0; \
00228 ph = &h[k*n_tap]; \
00229 pvect2 = &vect2[n + n_tap]; \
00230 for(i=0; i<n_tap - LOOP_UNROLL + 1; i += LOOP_UNROLL) \
00231 { \
00232 MREPEAT(LOOP_UNROLL, DSP16_RESAMPLING_FILTER, 0) \
00233 ph += LOOP_UNROLL; \
00234 pvect2 -= LOOP_UNROLL; \
00235 } \
00236 MREPEAT(x_num, DSP16_RESAMPLING_FILTER, 0); \
00237 *pvect1++ = sum >> DSP16_QB; \
00238 size++; \
00239 } \
00240 counter = k / interpolation_ratio; \
00241 n += counter; \
00242 counter = k - counter * interpolation_ratio; \
00243 } \
00244 *pcounter = counter; \
00245 *pn = n - vect2_size; \
00246 \
00247 return size; \
00248 }
00249
00250 #else // LOOP_UNROLL <= 4
00251
00252 #define DSP16_RESAMPLING_KERNEL_X_FCT(x_num, data) \
00253 static int TPASTE2(dsp16_resampling_kernel_x, x_num)(dsp16_t *vect1, dsp16_t *vect2, int vect2_size, dsp16_t *h, int n_tap, int interpolation_ratio, int decimation_ratio, int *pcounter, int *pn) \
00254 { \
00255 S32 sum = 0; \
00256 int i, k, n; \
00257 dsp16_t *ph; \
00258 dsp16_t *pvect1; \
00259 dsp16_t *pvect2; \
00260 int size = 0; \
00261 int counter; \
00262 \
00263 pvect1 = vect1; \
00264 \
00265 n = *pn; \
00266 counter = *pcounter; \
00267 vect2_size -= n_tap; \
00268 while(n < vect2_size) \
00269 { \
00270 pvect2 = &vect2[n + n_tap]; \
00271 for(k=counter; k<interpolation_ratio; k += decimation_ratio) \
00272 { \
00273 sum = 0; \
00274 ph = &h[k*n_tap]; \
00275 for(i=0; i<n_tap - LOOP_UNROLL + 1; i += LOOP_UNROLL) \
00276 { \
00277 MREPEAT(LOOP_UNROLL, DSP16_RESAMPLING_FILTER, i) \
00278 } \
00279 MREPEAT(x_num, DSP16_RESAMPLING_FILTER, i); \
00280 *pvect1++ = sum >> DSP16_QB; \
00281 size++; \
00282 } \
00283 counter = k / interpolation_ratio; \
00284 n += counter; \
00285 counter = k - counter * interpolation_ratio; \
00286 } \
00287 *pcounter = counter; \
00288 *pn = n - vect2_size; \
00289 \
00290 return size; \
00291 }
00292
00293 #endif // LOOP_UNROLL > 4
00294
00295 #define DSP16_RESAMPLING_NO_LOOP_KERNEL_X_FCT(x_num, data) \
00296 static int TPASTE2(dsp16_resampling_no_loop_kernel_x, x_num)(dsp16_t *vect1, dsp16_t *vect2, int vect2_size, dsp16_t *h, int n_tap, int interpolation_ratio, int decimation_ratio, int *pcounter, int *pn) \
00297 { \
00298 S32 sum = 0; \
00299 int k, n; \
00300 dsp16_t *ph; \
00301 dsp16_t *pvect1; \
00302 dsp16_t *pvect2; \
00303 int size = 0; \
00304 int counter; \
00305 \
00306 pvect1 = vect1; \
00307 \
00308 n = *pn; \
00309 counter = *pcounter; \
00310 vect2_size -= n_tap; \
00311 while(n < vect2_size) \
00312 { \
00313 pvect2 = &vect2[n + n_tap]; \
00314 for(k=counter; k<interpolation_ratio; k += decimation_ratio) \
00315 { \
00316 sum = 0; \
00317 ph = &h[k*n_tap]; \
00318 MREPEAT(x_num, DSP16_RESAMPLING_FILTER, 0) \
00319 *pvect1++ = sum >> DSP16_QB; \
00320 size++; \
00321 } \
00322 counter = k / interpolation_ratio; \
00323 n += counter; \
00324 counter = k - counter * interpolation_ratio; \
00325 } \
00326 *pcounter = counter; \
00327 *pn = n - vect2_size; \
00328 \
00329 return size; \
00330 }
00331
00332 static int dsp16_resampling_no_loop_kernel_x0(dsp16_t *vect1, dsp16_t *vect2, int vect2_size, dsp16_t *h, int n_tap, int interpolation_ratio, int decimation_ratio, int *pcounter, int *pn)
00333 {
00334 int k, n;
00335 dsp16_t *pvect1;
00336 dsp16_t *pvect2;
00337 int counter;
00338 int size = 0;
00339
00340 pvect1 = vect1;
00341
00342 n = *pn;
00343 counter = *pcounter;
00344 while(n < vect2_size)
00345 {
00346 pvect2 = &vect2[n];
00347 for(k=counter; k<interpolation_ratio; k += decimation_ratio)
00348 {
00349 *pvect1++ = *pvect2;
00350 size++;
00351 }
00352 counter = k / interpolation_ratio;
00353 n += counter;
00354 counter = k - counter * interpolation_ratio;
00355 }
00356 *pcounter = counter;
00357 *pn = n - vect2_size;
00358
00359 return size;
00360 }
00361 DSP16_RESAMPLING_NO_LOOP_KERNEL_X_FCT(1, )
00362 DSP16_RESAMPLING_NO_LOOP_KERNEL_X_FCT(2, )
00363 DSP16_RESAMPLING_NO_LOOP_KERNEL_X_FCT(3, )
00364 DSP16_RESAMPLING_NO_LOOP_KERNEL_X_FCT(4, )
00365 DSP16_RESAMPLING_NO_LOOP_KERNEL_X_FCT(5, )
00366 DSP16_RESAMPLING_NO_LOOP_KERNEL_X_FCT(6, )
00367
00368 DSP16_RESAMPLING_KERNEL_X_FCT(0, )
00369 DSP16_RESAMPLING_KERNEL_X_FCT(1, )
00370 DSP16_RESAMPLING_KERNEL_X_FCT(2, )
00371 DSP16_RESAMPLING_KERNEL_X_FCT(3, )
00372 DSP16_RESAMPLING_KERNEL_X_FCT(4, )
00373 DSP16_RESAMPLING_KERNEL_X_FCT(5, )
00374
00375 static void dsp16_resampling_polynomial_interpolation(dsp16_t y[4], dsp16_t output_y[NB_PTS_TO_INTERPOLATE])
00376 {
00377 dsp32_t f_x0_x1, f_x1_x2, f_x2_x3;
00378 dsp32_t f_x0_x1_x2, f_x1_x2_x3;
00379 dsp32_t f_x0_x1_x2_x3;
00380 int i;
00381 const int x0 = 0;
00382 const int x1 = 1;
00383 const int x2 = NB_PTS_TO_INTERPOLATE + 1;
00384 const int x3 = NB_PTS_TO_INTERPOLATE + 2;
00385
00386
00387 f_x0_x1 = ((dsp32_t) y[1] - y[0]) / (x1 - x0);
00388 f_x1_x2 = ((dsp32_t) y[2] - y[1]) / (x2 - x1);
00389 f_x2_x3 = ((dsp32_t) y[3] - y[2]) / (x3 - x2);
00390
00391 f_x0_x1_x2 = (f_x1_x2 - f_x0_x1) / (x2 - x0);
00392 f_x1_x2_x3 = (f_x2_x3 - f_x1_x2) / (x3 - x1);
00393
00394 f_x0_x1_x2_x3 = (f_x1_x2_x3 - f_x0_x1_x2) / (x3 - x0);
00395
00396
00397
00398
00399
00400
00401
00402
00403 for (i=0; i<NB_PTS_TO_INTERPOLATE; i++)
00404 {
00405 int e1, e2, e3;
00406 int x = i + 2;
00407 dsp32_t temp_y;
00408
00409 e1 = (x - x0);
00410 e2 = e1 * (x - x1);
00411 e3 = e2 * (x - x2);
00412
00413 temp_y = y[0] + f_x0_x1 * e1 + f_x0_x1_x2 * e2 + f_x0_x1_x2_x3 * e3;
00414 output_y[i] = (dsp16_t) temp_y;
00415 }
00416 }
00417
00418 void dsp16_resampling_compute(dsp_resampling_t *dsp_resampling, dsp16_t *output, dsp16_t *input, int channel)
00419 {
00420 int n_tap;
00421 dsp16_t *input_extended;
00422 dsp16_t link_y[4];
00423 typedef int (*resampling_kernel_opti_t)(dsp16_t *, dsp16_t *, int, dsp16_t *, int, int, int, int *, int *);
00424 static const resampling_kernel_opti_t resampling_end_kernel_opti[] = {
00425 MREPEAT(LOOP_UNROLL, DSP16_RESAMPLING_FUNCTION_NAME, )
00426 };
00427 static const resampling_kernel_opti_t resampling_no_loop_end_kernel_opti[] = {
00428 MREPEAT(LOOP_UNROLL_PLUS_ONE, DSP16_RESAMPLING_NO_LOOP_FUNCTION_NAME, )
00429 };
00430
00431 n_tap = dsp_resampling->filter_order;
00432 input_extended = ((dsp16_t *) dsp_resampling->last_samples) + dsp_resampling->buffer_size * channel;
00433
00434
00435 if (dsp_resampling->link_required)
00436 dsp16_vect_copy(link_y, &input_extended[n_tap], 2);
00437
00438
00439 dsp16_vect_copy(&input_extended[n_tap], input, dsp_resampling->buffer_size - n_tap);
00440
00441
00442 if (dsp_resampling->interpolation_factor == dsp_resampling->decimation_factor)
00443 {
00444 int i;
00445 dsp16_t *data;
00446 dsp16_t weight_x1, weight_x2;
00447
00448 data = &input_extended[(n_tap + 1)/2];
00449 weight_x1 = dsp_resampling->interpolation_factor - dsp_resampling->context[channel].input_buffer_pos;
00450 weight_x2 = dsp_resampling->context[channel].input_buffer_pos;
00451
00452 dsp_resampling->current_buffer_size = dsp_resampling->buffer_size - n_tap;
00453
00454 for (i=0; i<dsp_resampling->current_buffer_size; i++)
00455 output[i] = ((dsp32_t) (data[i]*weight_x1) + (data[i+1]*weight_x2)) / dsp_resampling->interpolation_factor;
00456 }
00457 else if (n_tap <= LOOP_UNROLL)
00458 {
00459 dsp_resampling->current_buffer_size = resampling_no_loop_end_kernel_opti[n_tap](
00460 output,
00461 input_extended,
00462 dsp_resampling->buffer_size,
00463 (dsp16_t *) dsp_resampling->fir_coefs,
00464 dsp_resampling->filter_order,
00465 dsp_resampling->interpolation_factor,
00466 dsp_resampling->decimation_factor,
00467 &dsp_resampling->context[channel].input_buffer_pos,
00468 &dsp_resampling->context[channel].index);
00469 }
00470 else
00471 {
00472 dsp_resampling->current_buffer_size = resampling_end_kernel_opti[n_tap%LOOP_UNROLL](
00473 output,
00474 input_extended,
00475 dsp_resampling->buffer_size,
00476 (dsp16_t *) dsp_resampling->fir_coefs,
00477 dsp_resampling->filter_order,
00478 dsp_resampling->interpolation_factor,
00479 dsp_resampling->decimation_factor,
00480 &dsp_resampling->context[channel].input_buffer_pos,
00481 &dsp_resampling->context[channel].index);
00482 }
00483
00484
00485 if (dsp_resampling->link_required)
00486 {
00487 link_y[2] = output[NB_PTS_TO_INTERPOLATE + 0];
00488 link_y[3] = output[NB_PTS_TO_INTERPOLATE + 1];
00489 dsp16_resampling_polynomial_interpolation(link_y, output);
00490
00491 dsp_resampling->link_required = false;
00492 }
00493
00494
00495 dsp16_vect_copy(input_extended, &input_extended[dsp_resampling->buffer_size - n_tap], n_tap);
00496
00497 dsp16_vect_copy(&input_extended[n_tap], &output[dsp_resampling->current_buffer_size - 2], 2);
00498 }
00499
00500 bool dsp16_resampling_link(dsp_resampling_t *dsp_resampling_output, dsp_resampling_t *dsp_resampling_input)
00501 {
00502 int channel;
00503 int n_tap_input, n_tap_output;
00504
00505
00506 n_tap_input = dsp_resampling_input->filter_order;
00507 n_tap_output = dsp_resampling_output->filter_order;
00508
00509
00510 if (n_tap_input != n_tap_output
00511 || dsp_resampling_output->buffer_size != dsp_resampling_input->buffer_size
00512 || dsp_resampling_output->nb_channels != dsp_resampling_input->nb_channels)
00513 return false;
00514
00515
00516 if (dsp_resampling_output->interpolation_factor == dsp_resampling_output->decimation_factor)
00517 {
00518 dsp_resampling_output->interpolation_factor = dsp_resampling_input->interpolation_factor;
00519 dsp_resampling_output->decimation_factor = dsp_resampling_input->interpolation_factor;
00520 }
00521
00522
00523 for (channel = 0; channel < dsp_resampling_output->nb_channels; channel++)
00524 {
00525 dsp16_t *input_extended_input = ((dsp16_t *) dsp_resampling_input->last_samples) + dsp_resampling_input->buffer_size * channel;
00526 dsp16_t *input_extended_output = ((dsp16_t *) dsp_resampling_output->last_samples) + dsp_resampling_output->buffer_size * channel;
00527
00528 dsp16_vect_copy(input_extended_output, input_extended_input, n_tap_input + 2);
00529
00530
00531 dsp_resampling_output->context[channel].index = dsp_resampling_input->context[channel].index;
00532 dsp_resampling_output->context[channel].input_buffer_pos = (dsp_resampling_input->context[channel].input_buffer_pos
00533 * dsp_resampling_output->interpolation_factor)
00534 / dsp_resampling_input->interpolation_factor;
00535 }
00536
00537
00538 dsp_resampling_output->link_required = true;
00539
00540 return true;
00541 }
00542
00543 int dsp16_resampling_get_output_current_buffer_size(dsp_resampling_t *dsp_resampling)
00544 {
00545 return dsp_resampling->current_buffer_size;
00546 }
00547
00548 int dsp16_resampling_get_output_max_buffer_size(dsp_resampling_t *dsp_resampling)
00549 {
00550 return (dsp_resampling->buffer_size * dsp_resampling->interpolation_factor - dsp_resampling->filter_order * dsp_resampling->interpolation_factor) / dsp_resampling->decimation_factor + 1;
00551 }
00552
00553 void dsp16_resampling_free(dsp_resampling_t *dsp_resampling, free_fct_t free_fct)
00554 {
00555 free_fct(dsp_resampling);
00556 }