static inline void
calc_output_quad (SINC_FILTER *filter, increment_t increment, increment_t start_filter_index, double scale, float * output)
{ double fraction, left [4], right [4], icoeff ;
increment_t filter_index, max_filter_index ;
int data_index, coeff_count, indx ;
/* Convert input parameters into fixed point. */
max_filter_index = int_to_fp (filter->coeff_half_len) ;
/* First apply the left half of the filter. */
filter_index = start_filter_index ;
coeff_count = (max_filter_index - filter_index) / increment ;
filter_index = filter_index + coeff_count * increment ;
data_index = filter->b_current - filter->channels * coeff_count ;
left [0] = left [1] = left [2] = left [3] = 0.0 ;
do
{ fraction = fp_to_double (filter_index) ;
indx = fp_to_int (filter_index) ;
icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;
left [0] += icoeff * filter->buffer [data_index] ;
left [1] += icoeff * filter->buffer [data_index + 1] ;
left [2] += icoeff * filter->buffer [data_index + 2] ;
left [3] += icoeff * filter->buffer [data_index + 3] ;
filter_index -= increment ;
data_index = data_index + 4 ;
}
while (filter_index >= MAKE_INCREMENT_T (0)) ;
/* Now apply the right half of the filter. */
filter_index = increment - start_filter_index ;
coeff_count = (max_filter_index - filter_index) / increment ;
filter_index = filter_index + coeff_count * increment ;
data_index = filter->b_current + filter->channels * (1 + coeff_count) ;
right [0] = right [1] = right [2] = right [3] = 0.0 ;
do
{ fraction = fp_to_double (filter_index) ;
indx = fp_to_int (filter_index) ;
icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;
right [0] += icoeff * filter->buffer [data_index] ;
right [1] += icoeff * filter->buffer [data_index + 1] ;
right [2] += icoeff * filter->buffer [data_index + 2] ;
right [3] += icoeff * filter->buffer [data_index + 3] ;
filter_index -= increment ;
data_index = data_index - 4 ;
}
while (filter_index > MAKE_INCREMENT_T (0)) ;
output [0] = scale * (left [0] + right [0]) ;
output [1] = scale * (left [1] + right [1]) ;
output [2] = scale * (left [2] + right [2]) ;
output [3] = scale * (left [3] + right [3]) ;
} /* calc_output_quad */
static inline double
calc_output_single (SINC_FILTER *filter, increment_t increment, increment_t start_filter_index)
{ double fraction, left, right, icoeff ;
increment_t filter_index, max_filter_index ;
int data_index, coeff_count, indx ;
/* Convert input parameters into fixed point. */
max_filter_index = int_to_fp (filter->coeff_half_len) ;
/* First apply the left half of the filter. */
filter_index = start_filter_index ;
coeff_count = (max_filter_index - filter_index) / increment ;
filter_index = filter_index + coeff_count * increment ;
data_index = filter->b_current - coeff_count ;
left = 0.0 ;
do
{ fraction = fp_to_double (filter_index) ;
indx = fp_to_int (filter_index) ;
icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;
left += icoeff * filter->buffer [data_index] ;
filter_index -= increment ;
data_index = data_index + 1 ;
}
while (filter_index >= MAKE_INCREMENT_T (0)) ;
/* Now apply the right half of the filter. */
filter_index = increment - start_filter_index ;
coeff_count = (max_filter_index - filter_index) / increment ;
filter_index = filter_index + coeff_count * increment ;
data_index = filter->b_current + 1 + coeff_count ;
right = 0.0 ;
do
{ fraction = fp_to_double (filter_index) ;
indx = fp_to_int (filter_index) ;
icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;
right += icoeff * filter->buffer [data_index] ;
filter_index -= increment ;
data_index = data_index - 1 ;
}
while (filter_index > MAKE_INCREMENT_T (0)) ;
return (left + right) ;
} /* calc_output_single */
static double
calc_output (SINC_FILTER *filter, increment_t increment, increment_t start_filter_index, int ch)
{ double fraction, left, right, icoeff ;
increment_t filter_index, max_filter_index ;
int data_index, coeff_count, indx ;
/* Convert input parameters into fixed point. */
max_filter_index = INT_TO_FP (filter->coeff_half_len) ;
/* First apply the left half of the filter. */
filter_index = start_filter_index ;
coeff_count = (max_filter_index - filter_index) / increment ;
filter_index = filter_index + coeff_count * increment ;
data_index = filter->b_current - filter->channels * coeff_count ;
left = 0.0 ;
do
{ fraction = FP_TO_DOUBLE (filter_index) ;
indx = FP_TO_INT (filter_index) ;
icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;
left += icoeff * filter->buffer [data_index + ch] ;
filter_index -= increment ;
data_index = data_index + filter->channels ;
}
while (filter_index >= MAKE_INCREMENT_T (0)) ;
/* Now apply the right half of the filter. */
filter_index = increment - start_filter_index ;
coeff_count = (max_filter_index - filter_index) / increment ;
filter_index = filter_index + coeff_count * increment ;
data_index = filter->b_current + filter->channels * (1 + coeff_count) ;
right = 0.0 ;
do
{ fraction = FP_TO_DOUBLE (filter_index) ;
indx = FP_TO_INT (filter_index) ;
icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;
right += icoeff * filter->buffer [data_index + ch] ;
filter_index -= increment ;
data_index = data_index - filter->channels ;
}
while (filter_index > MAKE_INCREMENT_T (0)) ;
return (left + right) ;
} /* calc_output */
static inline void
calc_output_multi (SINC_FILTER *filter, increment_t increment, increment_t start_filter_index, int channels, double scale, float * output)
{ double fraction, icoeff ;
/* The following line is 1999 ISO Standard C. If your compiler complains, get a better compiler. */
double *left, *right ;
increment_t filter_index, max_filter_index ;
int data_index, coeff_count, indx, ch ;
left = filter->left_calc ;
right = filter->right_calc ;
/* Convert input parameters into fixed point. */
max_filter_index = int_to_fp (filter->coeff_half_len) ;
/* First apply the left half of the filter. */
filter_index = start_filter_index ;
coeff_count = (max_filter_index - filter_index) / increment ;
filter_index = filter_index + coeff_count * increment ;
data_index = filter->b_current - channels * coeff_count ;
memset (left, 0, sizeof (left [0]) * channels) ;
do
{ fraction = fp_to_double (filter_index) ;
indx = fp_to_int (filter_index) ;
icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;
/*
** Duff's Device.
** See : http://en.wikipedia.org/wiki/Duff's_device
*/
ch = channels ;
do
{
switch (ch % 8)
{ default :
ch -- ;
left [ch] += icoeff * filter->buffer [data_index + ch] ;
case 7 :
ch -- ;
left [ch] += icoeff * filter->buffer [data_index + ch] ;
case 6 :
ch -- ;
left [ch] += icoeff * filter->buffer [data_index + ch] ;
case 5 :
ch -- ;
left [ch] += icoeff * filter->buffer [data_index + ch] ;
case 4 :
ch -- ;
left [ch] += icoeff * filter->buffer [data_index + ch] ;
case 3 :
ch -- ;
left [ch] += icoeff * filter->buffer [data_index + ch] ;
case 2 :
ch -- ;
left [ch] += icoeff * filter->buffer [data_index + ch] ;
case 1 :
ch -- ;
left [ch] += icoeff * filter->buffer [data_index + ch] ;
} ;
}
while (ch > 0) ;
filter_index -= increment ;
data_index = data_index + channels ;
}
while (filter_index >= MAKE_INCREMENT_T (0)) ;
/* Now apply the right half of the filter. */
filter_index = increment - start_filter_index ;
coeff_count = (max_filter_index - filter_index) / increment ;
filter_index = filter_index + coeff_count * increment ;
data_index = filter->b_current + channels * (1 + coeff_count) ;
memset (right, 0, sizeof (right [0]) * channels) ;
do
{ fraction = fp_to_double (filter_index) ;
indx = fp_to_int (filter_index) ;
icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;
ch = channels ;
do
{
switch (ch % 8)
{ default :
ch -- ;
right [ch] += icoeff * filter->buffer [data_index + ch] ;
case 7 :
ch -- ;
right [ch] += icoeff * filter->buffer [data_index + ch] ;
case 6 :
ch -- ;
right [ch] += icoeff * filter->buffer [data_index + ch] ;
case 5 :
ch -- ;
right [ch] += icoeff * filter->buffer [data_index + ch] ;
case 4 :
ch -- ;
right [ch] += icoeff * filter->buffer [data_index + ch] ;
case 3 :
ch -- ;
right [ch] += icoeff * filter->buffer [data_index + ch] ;
case 2 :
ch -- ;
right [ch] += icoeff * filter->buffer [data_index + ch] ;
case 1 :
ch -- ;
right [ch] += icoeff * filter->buffer [data_index + ch] ;
} ;
}
while (ch > 0) ;
filter_index -= increment ;
data_index = data_index - channels ;
}
while (filter_index > MAKE_INCREMENT_T (0)) ;
ch = channels ;
do
{
switch (ch % 8)
{ default :
ch -- ;
output [ch] = scale * (left [ch] + right [ch]) ;
case 7 :
ch -- ;
output [ch] = scale * (left [ch] + right [ch]) ;
case 6 :
ch -- ;
output [ch] = scale * (left [ch] + right [ch]) ;
case 5 :
ch -- ;
output [ch] = scale * (left [ch] + right [ch]) ;
case 4 :
ch -- ;
output [ch] = scale * (left [ch] + right [ch]) ;
case 3 :
ch -- ;
output [ch] = scale * (left [ch] + right [ch]) ;
case 2 :
ch -- ;
output [ch] = scale * (left [ch] + right [ch]) ;
case 1 :
ch -- ;
output [ch] = scale * (left [ch] + right [ch]) ;
} ;
}
while (ch > 0) ;
return ;
} /* calc_output_multi */
int
sinc_set_converter (SRC_PRIVATE *psrc, int src_enum)
{ SINC_FILTER *filter, temp_filter ;
increment_t count ;
int bits ;
/* Quick sanity check. */
if (SHIFT_BITS >= sizeof (increment_t) * 8 - 1)
return SRC_ERR_SHIFT_BITS ;
if (psrc->private_data != NULL)
{ free (psrc->private_data) ;
psrc->private_data = NULL ;
} ;
memset (&temp_filter, 0, sizeof (temp_filter)) ;
temp_filter.sinc_magic_marker = SINC_MAGIC_MARKER ;
temp_filter.channels = psrc->channels ;
if (psrc->channels > ARRAY_LEN (temp_filter.left_calc))
return SRC_ERR_BAD_CHANNEL_COUNT ;
else if (psrc->channels == 1)
{ psrc->const_process = sinc_mono_vari_process ;
psrc->vari_process = sinc_mono_vari_process ;
}
else
if (psrc->channels == 2)
{ psrc->const_process = sinc_stereo_vari_process ;
psrc->vari_process = sinc_stereo_vari_process ;
}
else
if (psrc->channels == 4)
{ psrc->const_process = sinc_quad_vari_process ;
psrc->vari_process = sinc_quad_vari_process ;
}
else
if (psrc->channels == 6)
{ psrc->const_process = sinc_hex_vari_process ;
psrc->vari_process = sinc_hex_vari_process ;
}
else
{ psrc->const_process = sinc_multichan_vari_process ;
psrc->vari_process = sinc_multichan_vari_process ;
} ;
psrc->reset = sinc_reset ;
switch (src_enum)
{ case SRC_SINC_FASTEST :
temp_filter.coeffs = fastest_coeffs.coeffs ;
temp_filter.coeff_half_len = ARRAY_LEN (fastest_coeffs.coeffs) - 2 ;
temp_filter.index_inc = fastest_coeffs.increment ;
break ;
case SRC_SINC_MEDIUM_QUALITY :
temp_filter.coeffs = slow_mid_qual_coeffs.coeffs ;
temp_filter.coeff_half_len = ARRAY_LEN (slow_mid_qual_coeffs.coeffs) - 2 ;
temp_filter.index_inc = slow_mid_qual_coeffs.increment ;
break ;
case SRC_SINC_BEST_QUALITY :
temp_filter.coeffs = slow_high_qual_coeffs.coeffs ;
temp_filter.coeff_half_len = ARRAY_LEN (slow_high_qual_coeffs.coeffs) - 2 ;
temp_filter.index_inc = slow_high_qual_coeffs.increment ;
break ;
default :
return SRC_ERR_BAD_CONVERTER ;
} ;
/*
** FIXME : This needs to be looked at more closely to see if there is
** a better way. Need to look at prepare_data () at the same time.
*/
temp_filter.b_len = lrint (2.5 * temp_filter.coeff_half_len / (temp_filter.index_inc * 1.0) * SRC_MAX_RATIO) ;
temp_filter.b_len = MAX (temp_filter.b_len, 4096) ;
temp_filter.b_len *= temp_filter.channels ;
if ((filter = calloc (1, sizeof (SINC_FILTER) + sizeof (filter->buffer [0]) * (temp_filter.b_len + temp_filter.channels))) == NULL)
return SRC_ERR_MALLOC_FAILED ;
*filter = temp_filter ;
memset (&temp_filter, 0xEE, sizeof (temp_filter)) ;
psrc->private_data = filter ;
sinc_reset (psrc) ;
count = filter->coeff_half_len ;
for (bits = 0 ; (MAKE_INCREMENT_T (1) << bits) < count ; bits++)
count |= (MAKE_INCREMENT_T (1) << bits) ;
if (bits + SHIFT_BITS - 1 >= (int) (sizeof (increment_t) * 8))
return SRC_ERR_FILTER_LEN ;
return SRC_ERR_NO_ERROR ;
} /* sinc_set_converter */
static inline double
calc_output_single (SINC_FILTER *filter, const increment_t increment, const increment_t start_filter_index)
{
#ifdef RESAMPLER_SSE_OPT
__m128i increment4;
__m128 left128,right128;
float left,right;
#else
double left,right;
#endif
const coeff_t * const __restrict coeffs = filter->coeffs;
const float * const __restrict buffer = filter->buffer;
increment_t filter_index, max_filter_index ;
int data_index, coeff_count;
/* Convert input parameters into fixed point. */
max_filter_index = int_to_fp (filter->coeff_half_len) ;
/* First apply the left half of the filter. */
filter_index = start_filter_index ;
coeff_count = (max_filter_index - filter_index) / increment ;
filter_index = filter_index + coeff_count * increment ;
data_index = filter->b_current - coeff_count ;
#ifdef RESAMPLER_SSE_OPT
increment4 = _mm_set_epi32(increment * 3, increment * 2, increment, 0);
left128 = _mm_setzero_ps();
while(filter_index >= increment * 3)
{
#ifdef USE_WINDOWS_CODE
__m128i indx = _mm_sub_epi32(_mm_set1_epi32(filter_index), increment4);
__m128i fractioni = _mm_and_si128(indx,_mm_set1_epi32(((((increment_t)1) << SHIFT_BITS) - 1)));
#else
Windows__m128i indx;
indx.m128i = _mm_sub_epi32(_mm_set1_epi32(filter_index), increment4);
__m128i fractioni = _mm_and_si128(indx.m128i,_mm_set1_epi32(((((increment_t)1) << SHIFT_BITS) - 1)));
#endif
__m128 icoeff0, icoeff2; // warning that these are uninitialized is okay and its intended, as both high and low 64bit-parts are set below
__m128 icoeff,icoeffp1,icoeffd,fraction;
#ifdef _DEBUG
icoeff0 = icoeff2 = _mm_setzero_ps();
#endif
#ifdef USE_WINDOWS_CODE
indx = _mm_srai_epi32(indx, SHIFT_BITS);
#else
indx.m128i = _mm_srai_epi32(indx.m128i, SHIFT_BITS);
#endif
icoeff0 = _mm_loadh_pi(_mm_loadl_pi(icoeff0, (__m64*)(coeffs + indx.m128i_i32[0])), (__m64*)(coeffs + indx.m128i_i32[1]));
icoeff2 = _mm_loadh_pi(_mm_loadl_pi(icoeff2, (__m64*)(coeffs + indx.m128i_i32[2])), (__m64*)(coeffs + indx.m128i_i32[3]));
icoeff = _mm_shuffle_ps(icoeff0, icoeff2, _MM_SHUFFLE(2, 0, 2, 0));
icoeffp1 = _mm_shuffle_ps(icoeff0, icoeff2, _MM_SHUFFLE(3, 1, 3, 1));
icoeffd = _mm_sub_ps(icoeffp1, icoeff);
fraction = _mm_mul_ps(_mm_cvtepi32_ps(fractioni), _mm_set1_ps((float)INV_FP_ONE));
icoeff = _mm_add_ps(icoeff,_mm_mul_ps(icoeffd, fraction));
left128 = _mm_add_ps(left128,_mm_mul_ps(icoeff, _mm_loadu_ps(buffer + data_index)));
data_index += 4;
filter_index -= increment * 4;
}
#endif
left = 0.;
while (filter_index >= MAKE_INCREMENT_T(0))
{
coeff_t fraction = fp_to_float(filter_index);
int indx = fp_to_int(filter_index);
coeff_t icoeff = coeffs[indx] + fraction * (coeffs[indx + 1] - coeffs[indx]);
left += icoeff * buffer[data_index];
filter_index -= increment;
data_index++;
}
/* Now apply the right half of the filter. */
filter_index = increment - start_filter_index ;
coeff_count = (max_filter_index - filter_index) / increment ;
filter_index = filter_index + coeff_count * increment ;
data_index = filter->b_current + 1 + coeff_count ;
#ifdef RESAMPLER_SSE_OPT
right128 = _mm_setzero_ps();
while (filter_index > increment * 3)
{
#ifdef USE_WINDOWS_CODE
__m128i indx = _mm_sub_epi32(_mm_set1_epi32(filter_index), increment4);
__m128i fractioni = _mm_and_si128(indx, _mm_set1_epi32(((((increment_t)1) << SHIFT_BITS) - 1)));
#else
Windows__m128i indx;
indx.m128i = _mm_sub_epi32(_mm_set1_epi32(filter_index), increment4);
__m128i fractioni = _mm_and_si128(indx.m128i, _mm_set1_epi32(((((increment_t)1) << SHIFT_BITS) - 1)));
#endif
__m128 icoeff0, icoeff2; // warning that these are uninitialized is okay and its intended, as both high and low 64bit-parts are set below
__m128 icoeff,icoeffp1,icoeffd,fraction,data;
#ifdef _DEBUG
icoeff0 = icoeff2 = _mm_setzero_ps();
#endif
#ifdef USE_WINDOWS_CODE
indx = _mm_srai_epi32(indx, SHIFT_BITS);
#else
indx.m128i = _mm_srai_epi32(indx.m128i, SHIFT_BITS);
#endif
icoeff0 = _mm_loadh_pi(_mm_loadl_pi(icoeff0, (__m64*)(coeffs + indx.m128i_i32[0])), (__m64*)(coeffs + indx.m128i_i32[1]));
icoeff2 = _mm_loadh_pi(_mm_loadl_pi(icoeff2, (__m64*)(coeffs + indx.m128i_i32[2])), (__m64*)(coeffs + indx.m128i_i32[3]));
icoeff = _mm_shuffle_ps(icoeff0, icoeff2, _MM_SHUFFLE(2, 0, 2, 0));
icoeffp1 = _mm_shuffle_ps(icoeff0, icoeff2, _MM_SHUFFLE(3, 1, 3, 1));
icoeffd = _mm_sub_ps(icoeffp1, icoeff);
fraction = _mm_mul_ps(_mm_cvtepi32_ps(fractioni), _mm_set1_ps((float)INV_FP_ONE));
icoeff = _mm_add_ps(icoeff, _mm_mul_ps(icoeffd, fraction));
data = _mm_loadu_ps(buffer + (data_index - 3));
right128 = _mm_add_ps(right128,_mm_mul_ps(icoeff, _mm_shuffle_ps(data,data,_MM_SHUFFLE(0,1,2,3))));
data_index -= 4;
filter_index -= increment * 4;
}
#endif
right = 0.;
while (filter_index > MAKE_INCREMENT_T(0))
{
coeff_t fraction = fp_to_float(filter_index);
int indx = fp_to_int(filter_index);
coeff_t icoeff = coeffs[indx] + fraction * (coeffs[indx + 1] - coeffs[indx]);
right += icoeff * buffer[data_index];
filter_index -= increment;
data_index--;
}
return (
#ifdef RESAMPLER_SSE_OPT
_mm_cvtss_f32(horizontal_add(left128)) + _mm_cvtss_f32(horizontal_add(right128)) +
#endif
left + right) ;
} /* calc_output_single */
