mirror of
https://github.com/portapack-mayhem/mayhem-firmware.git
synced 2026-09-07 07:19:07 +00:00
Fixed module loading (again), only audio tx works for now
This commit is contained in:
furrtek
@@ -26,7 +26,451 @@
|
||||
namespace dsp {
|
||||
namespace decimate {
|
||||
|
||||
buffer_c16_t TranslateByFSOver4AndDecimateBy2CIC3::execute(buffer_c8_t src, buffer_c16_t dst) {
|
||||
static inline complex32_t mac_fs4_shift(
|
||||
const vec2_s16* const z,
|
||||
const vec2_s16* const t,
|
||||
const size_t index,
|
||||
const complex32_t accum
|
||||
) {
|
||||
/* Accumulate sample * tap results for samples already in z buffer.
|
||||
* Multiply using swap/negation to achieve Fs/4 shift.
|
||||
* For iterations where samples are shifting out of z buffer (being discarded).
|
||||
* Expect negated tap t[2] to accomodate instruction set limitations.
|
||||
*/
|
||||
const bool negated_t2 = index & 1;
|
||||
const auto q1_i0 = z[index*2 + 0];
|
||||
const auto i1_q0 = z[index*2 + 1];
|
||||
const auto t1_t0 = t[index];
|
||||
const auto real = negated_t2 ? smlsd(q1_i0, t1_t0, accum.real()) : smlad(q1_i0, t1_t0, accum.real());
|
||||
const auto imag = negated_t2 ? smlad(i1_q0, t1_t0, accum.imag()) : smlsd(i1_q0, t1_t0, accum.imag());
|
||||
return { real, imag };
|
||||
}
|
||||
|
||||
static inline complex32_t mac_shift(
|
||||
const vec2_s16* const z,
|
||||
const vec2_s16* const t,
|
||||
const size_t index,
|
||||
const complex32_t accum
|
||||
) {
|
||||
/* Accumulate sample * tap results for samples already in z buffer.
|
||||
* For iterations where samples are shifting out of z buffer (being discarded).
|
||||
* real += i1 * t1 + i0 * t0
|
||||
* imag += q1 * t1 + q0 * t0
|
||||
*/
|
||||
const auto i1_i0 = z[index*2 + 0];
|
||||
const auto q1_q0 = z[index*2 + 1];
|
||||
const auto t1_t0 = t[index];
|
||||
const auto real = smlad(i1_i0, t1_t0, accum.real());
|
||||
const auto imag = smlad(q1_q0, t1_t0, accum.imag());
|
||||
return { real, imag };
|
||||
}
|
||||
|
||||
static inline complex32_t mac_fs4_shift_and_store(
|
||||
vec2_s16* const z,
|
||||
const vec2_s16* const t,
|
||||
const size_t decimation_factor,
|
||||
const size_t index,
|
||||
const complex32_t accum
|
||||
) {
|
||||
/* Accumulate sample * tap results for samples already in z buffer.
|
||||
* Place new samples into z buffer.
|
||||
* Expect negated tap t[2] to accomodate instruction set limitations.
|
||||
*/
|
||||
const bool negated_t2 = index & 1;
|
||||
const auto q1_i0 = z[decimation_factor + index*2 + 0];
|
||||
const auto i1_q0 = z[decimation_factor + index*2 + 1];
|
||||
const auto t1_t0 = t[decimation_factor / 2 + index];
|
||||
z[index*2 + 0] = q1_i0;
|
||||
const auto real = negated_t2 ? smlsd(q1_i0, t1_t0, accum.real()) : smlad(q1_i0, t1_t0, accum.real());
|
||||
z[index*2 + 1] = i1_q0;
|
||||
const auto imag = negated_t2 ? smlad(i1_q0, t1_t0, accum.imag()) : smlsd(i1_q0, t1_t0, accum.imag());
|
||||
return { real, imag };
|
||||
}
|
||||
|
||||
static inline complex32_t mac_shift_and_store(
|
||||
vec2_s16* const z,
|
||||
const vec2_s16* const t,
|
||||
const size_t decimation_factor,
|
||||
const size_t index,
|
||||
const complex32_t accum
|
||||
) {
|
||||
/* Accumulate sample * tap results for samples already in z buffer.
|
||||
* Place new samples into z buffer.
|
||||
* Expect negated tap t[2] to accomodate instruction set limitations.
|
||||
*/
|
||||
const auto i1_i0 = z[decimation_factor + index*2 + 0];
|
||||
const auto q1_q0 = z[decimation_factor + index*2 + 1];
|
||||
const auto t1_t0 = t[decimation_factor / 2 + index];
|
||||
z[index*2 + 0] = i1_i0;
|
||||
const auto real = smlad(i1_i0, t1_t0, accum.real());
|
||||
z[index*2 + 1] = q1_q0;
|
||||
const auto imag = smlad(q1_q0, t1_t0, accum.imag());
|
||||
return { real, imag };
|
||||
}
|
||||
|
||||
static inline complex32_t mac_fs4_shift_and_store_new_c8_samples(
|
||||
vec2_s16* const z,
|
||||
const vec2_s16* const t,
|
||||
const vec4_s8* const in,
|
||||
const size_t decimation_factor,
|
||||
const size_t index,
|
||||
const size_t length,
|
||||
const complex32_t accum
|
||||
) {
|
||||
/* Accumulate sample * tap results for new samples.
|
||||
* Place new samples into z buffer.
|
||||
* Expect negated tap t[2] to accomodate instruction set limitations.
|
||||
*/
|
||||
const bool negated_t2 = index & 1;
|
||||
const auto q1_i1_q0_i0 = in[index];
|
||||
const auto t1_t0 = t[(length - decimation_factor) / 2 + index];
|
||||
const auto i1_q1_i0_q0 = rev16(q1_i1_q0_i0);
|
||||
const auto i1_q1_q0_i0 = pkhbt(q1_i1_q0_i0, i1_q1_i0_q0);
|
||||
const auto q1_i0 = sxtb16(i1_q1_q0_i0);
|
||||
const auto i1_q0 = sxtb16(i1_q1_q0_i0, 8);
|
||||
z[length - decimation_factor * 2 + index*2 + 0] = q1_i0;
|
||||
const auto real = negated_t2 ? smlsd(q1_i0, t1_t0, accum.real()) : smlad(q1_i0, t1_t0, accum.real());
|
||||
z[length - decimation_factor * 2 + index*2 + 1] = i1_q0;
|
||||
const auto imag = negated_t2 ? smlad(i1_q0, t1_t0, accum.imag()) : smlsd(i1_q0, t1_t0, accum.imag());
|
||||
return { real, imag };
|
||||
}
|
||||
|
||||
static inline complex32_t mac_shift_and_store_new_c16_samples(
|
||||
vec2_s16* const z,
|
||||
const vec2_s16* const t,
|
||||
const vec2_s16* const in,
|
||||
const size_t decimation_factor,
|
||||
const size_t index,
|
||||
const size_t length,
|
||||
const complex32_t accum
|
||||
) {
|
||||
/* Accumulate sample * tap results for new samples.
|
||||
* Place new samples into z buffer.
|
||||
* Expect negated tap t[2] to accomodate instruction set limitations.
|
||||
*/
|
||||
const auto q0_i0 = in[index*2+0];
|
||||
const auto q1_i1 = in[index*2+1];
|
||||
const auto i1_i0 = pkhbt(q0_i0, q1_i1, 16);
|
||||
const auto q1_q0 = pkhtb(q1_i1, q0_i0, 16);
|
||||
const auto t1_t0 = t[(length - decimation_factor) / 2 + index];
|
||||
z[length - decimation_factor * 2 + index*2 + 0] = i1_i0;
|
||||
const auto real = smlad(i1_i0, t1_t0, accum.real());
|
||||
z[length - decimation_factor * 2 + index*2 + 1] = q1_q0;
|
||||
const auto imag = smlad(q1_q0, t1_t0, accum.imag());
|
||||
return { real, imag };
|
||||
}
|
||||
|
||||
static inline uint32_t scale_round_and_pack(
|
||||
const complex32_t value,
|
||||
const int32_t scale_factor
|
||||
) {
|
||||
/* Multiply 32-bit components of the complex<int32_t> by a scale factor,
|
||||
* into int64_ts, then round to nearest LSB (1 << 32), saturate to 16 bits,
|
||||
* and pack into a complex<int16_t>.
|
||||
*/
|
||||
const auto scaled_real = __SMMULR(value.real(), scale_factor);
|
||||
const auto saturated_real = __SSAT(scaled_real, 16);
|
||||
|
||||
const auto scaled_imag = __SMMULR(value.imag(), scale_factor);
|
||||
const auto saturated_imag = __SSAT(scaled_imag, 16);
|
||||
|
||||
return __PKHBT(saturated_real, saturated_imag, 16);
|
||||
}
|
||||
|
||||
template<typename Tap>
|
||||
static void taps_copy(
|
||||
const Tap* const source,
|
||||
Tap* const target,
|
||||
const size_t count,
|
||||
const bool shift_up
|
||||
) {
|
||||
const uint32_t negate_pattern = shift_up ? 0b1110 : 0b0100;
|
||||
for(size_t i=0; i<count; i++) {
|
||||
const bool negate = (negate_pattern >> (i & 3)) & 1;
|
||||
target[i] = negate ? -source[i] : source[i];
|
||||
}
|
||||
}
|
||||
|
||||
// FIRC8xR16x24FS4Decim4 //////////////////////////////////////////////////
|
||||
|
||||
void FIRC8xR16x24FS4Decim4::configure(
|
||||
const std::array<tap_t, taps_count>& taps,
|
||||
const int32_t scale,
|
||||
const Shift shift
|
||||
) {
|
||||
taps_copy(taps.data(), taps_.data(), taps_.size(), shift == Shift::Up);
|
||||
output_scale = scale;
|
||||
z_.fill({});
|
||||
}
|
||||
|
||||
buffer_c16_t FIRC8xR16x24FS4Decim4::execute(
|
||||
const buffer_c8_t& src,
|
||||
const buffer_c16_t& dst
|
||||
) {
|
||||
vec2_s16* const z = static_cast<vec2_s16*>(__builtin_assume_aligned(z_.data(), 4));
|
||||
const vec2_s16* const t = static_cast<vec2_s16*>(__builtin_assume_aligned(taps_.data(), 4));
|
||||
uint32_t* const d = static_cast<uint32_t*>(__builtin_assume_aligned(dst.p, 4));
|
||||
|
||||
const auto k = output_scale;
|
||||
|
||||
const size_t count = src.count / decimation_factor;
|
||||
for(size_t i=0; i<count; i++) {
|
||||
const vec4_s8* const in = static_cast<const vec4_s8*>(__builtin_assume_aligned(&src.p[i * decimation_factor], 4));
|
||||
|
||||
complex32_t accum;
|
||||
|
||||
// Oldest samples are discarded.
|
||||
accum = mac_fs4_shift(z, t, 0, accum);
|
||||
accum = mac_fs4_shift(z, t, 1, accum);
|
||||
|
||||
// Middle samples are shifted earlier in the "z" delay buffer.
|
||||
accum = mac_fs4_shift_and_store(z, t, decimation_factor, 0, accum);
|
||||
accum = mac_fs4_shift_and_store(z, t, decimation_factor, 1, accum);
|
||||
accum = mac_fs4_shift_and_store(z, t, decimation_factor, 2, accum);
|
||||
accum = mac_fs4_shift_and_store(z, t, decimation_factor, 3, accum);
|
||||
accum = mac_fs4_shift_and_store(z, t, decimation_factor, 4, accum);
|
||||
accum = mac_fs4_shift_and_store(z, t, decimation_factor, 5, accum);
|
||||
accum = mac_fs4_shift_and_store(z, t, decimation_factor, 6, accum);
|
||||
accum = mac_fs4_shift_and_store(z, t, decimation_factor, 7, accum);
|
||||
|
||||
// Newest samples come from "in" buffer, are copied to "z" delay buffer.
|
||||
accum = mac_fs4_shift_and_store_new_c8_samples(z, t, in, decimation_factor, 0, taps_count, accum);
|
||||
accum = mac_fs4_shift_and_store_new_c8_samples(z, t, in, decimation_factor, 1, taps_count, accum);
|
||||
|
||||
d[i] = scale_round_and_pack(accum, k);
|
||||
}
|
||||
|
||||
return {
|
||||
dst.p,
|
||||
count,
|
||||
src.sampling_rate / decimation_factor
|
||||
};
|
||||
}
|
||||
|
||||
// FIRC8xR16x24FS4Decim8 //////////////////////////////////////////////////
|
||||
|
||||
void FIRC8xR16x24FS4Decim8::configure(
|
||||
const std::array<tap_t, taps_count>& taps,
|
||||
const int32_t scale,
|
||||
const Shift shift
|
||||
) {
|
||||
taps_copy(taps.data(), taps_.data(), taps_.size(), shift == Shift::Up);
|
||||
output_scale = scale;
|
||||
z_.fill({});
|
||||
}
|
||||
|
||||
buffer_c16_t FIRC8xR16x24FS4Decim8::execute(
|
||||
const buffer_c8_t& src,
|
||||
const buffer_c16_t& dst
|
||||
) {
|
||||
vec2_s16* const z = static_cast<vec2_s16*>(__builtin_assume_aligned(z_.data(), 4));
|
||||
const vec2_s16* const t = static_cast<vec2_s16*>(__builtin_assume_aligned(taps_.data(), 4));
|
||||
uint32_t* const d = static_cast<uint32_t*>(__builtin_assume_aligned(dst.p, 4));
|
||||
|
||||
const auto k = output_scale;
|
||||
|
||||
const size_t count = src.count / decimation_factor;
|
||||
for(size_t i=0; i<count; i++) {
|
||||
const vec4_s8* const in = static_cast<const vec4_s8*>(__builtin_assume_aligned(&src.p[i * decimation_factor], 4));
|
||||
|
||||
complex32_t accum;
|
||||
|
||||
// Oldest samples are discarded.
|
||||
accum = mac_fs4_shift(z, t, 0, accum);
|
||||
accum = mac_fs4_shift(z, t, 1, accum);
|
||||
accum = mac_fs4_shift(z, t, 2, accum);
|
||||
accum = mac_fs4_shift(z, t, 3, accum);
|
||||
|
||||
// Middle samples are shifted earlier in the "z" delay buffer.
|
||||
accum = mac_fs4_shift_and_store(z, t, decimation_factor, 0, accum);
|
||||
accum = mac_fs4_shift_and_store(z, t, decimation_factor, 1, accum);
|
||||
accum = mac_fs4_shift_and_store(z, t, decimation_factor, 2, accum);
|
||||
accum = mac_fs4_shift_and_store(z, t, decimation_factor, 3, accum);
|
||||
|
||||
// Newest samples come from "in" buffer, are copied to "z" delay buffer.
|
||||
accum = mac_fs4_shift_and_store_new_c8_samples(z, t, in, decimation_factor, 0, taps_count, accum);
|
||||
accum = mac_fs4_shift_and_store_new_c8_samples(z, t, in, decimation_factor, 1, taps_count, accum);
|
||||
accum = mac_fs4_shift_and_store_new_c8_samples(z, t, in, decimation_factor, 2, taps_count, accum);
|
||||
accum = mac_fs4_shift_and_store_new_c8_samples(z, t, in, decimation_factor, 3, taps_count, accum);
|
||||
|
||||
d[i] = scale_round_and_pack(accum, k);
|
||||
}
|
||||
|
||||
return {
|
||||
dst.p,
|
||||
count,
|
||||
src.sampling_rate / decimation_factor
|
||||
};
|
||||
}
|
||||
|
||||
// FIRC16xR16x16Decim2 ////////////////////////////////////////////////////
|
||||
|
||||
void FIRC16xR16x16Decim2::configure(
|
||||
const std::array<tap_t, taps_count>& taps,
|
||||
const int32_t scale
|
||||
) {
|
||||
std::copy(taps.cbegin(), taps.cend(), taps_.begin());
|
||||
output_scale = scale;
|
||||
z_.fill({});
|
||||
}
|
||||
|
||||
buffer_c16_t FIRC16xR16x16Decim2::execute(
|
||||
const buffer_c16_t& src,
|
||||
const buffer_c16_t& dst
|
||||
) {
|
||||
vec2_s16* const z = static_cast<vec2_s16*>(__builtin_assume_aligned(z_.data(), 4));
|
||||
const vec2_s16* const t = static_cast<vec2_s16*>(__builtin_assume_aligned(taps_.data(), 4));
|
||||
uint32_t* const d = static_cast<uint32_t*>(__builtin_assume_aligned(dst.p, 4));
|
||||
|
||||
const auto k = output_scale;
|
||||
|
||||
const size_t count = src.count / decimation_factor;
|
||||
for(size_t i=0; i<count; i++) {
|
||||
const vec2_s16* const in = static_cast<const vec2_s16*>(__builtin_assume_aligned(&src.p[i * decimation_factor], 4));
|
||||
|
||||
complex32_t accum;
|
||||
|
||||
// Oldest samples are discarded.
|
||||
accum = mac_shift(z, t, 0, accum);
|
||||
|
||||
// Middle samples are shifted earlier in the "z" delay buffer.
|
||||
accum = mac_shift_and_store(z, t, decimation_factor, 0, accum);
|
||||
accum = mac_shift_and_store(z, t, decimation_factor, 1, accum);
|
||||
accum = mac_shift_and_store(z, t, decimation_factor, 2, accum);
|
||||
accum = mac_shift_and_store(z, t, decimation_factor, 3, accum);
|
||||
accum = mac_shift_and_store(z, t, decimation_factor, 4, accum);
|
||||
accum = mac_shift_and_store(z, t, decimation_factor, 5, accum);
|
||||
|
||||
// Newest samples come from "in" buffer, are copied to "z" delay buffer.
|
||||
accum = mac_shift_and_store_new_c16_samples(z, t, in, decimation_factor, 0, taps_count, accum);
|
||||
|
||||
d[i] = scale_round_and_pack(accum, k);
|
||||
}
|
||||
|
||||
return {
|
||||
dst.p,
|
||||
count,
|
||||
src.sampling_rate / decimation_factor
|
||||
};
|
||||
}
|
||||
|
||||
// FIRC16xR16x32Decim8 ////////////////////////////////////////////////////
|
||||
|
||||
void FIRC16xR16x32Decim8::configure(
|
||||
const std::array<tap_t, taps_count>& taps,
|
||||
const int32_t scale
|
||||
) {
|
||||
std::copy(taps.cbegin(), taps.cend(), taps_.begin());
|
||||
output_scale = scale;
|
||||
z_.fill({});
|
||||
}
|
||||
|
||||
buffer_c16_t FIRC16xR16x32Decim8::execute(
|
||||
const buffer_c16_t& src,
|
||||
const buffer_c16_t& dst
|
||||
) {
|
||||
vec2_s16* const z = static_cast<vec2_s16*>(__builtin_assume_aligned(z_.data(), 4));
|
||||
const vec2_s16* const t = static_cast<vec2_s16*>(__builtin_assume_aligned(taps_.data(), 4));
|
||||
uint32_t* const d = static_cast<uint32_t*>(__builtin_assume_aligned(dst.p, 4));
|
||||
|
||||
const auto k = output_scale;
|
||||
|
||||
const size_t count = src.count / decimation_factor;
|
||||
for(size_t i=0; i<count; i++) {
|
||||
const vec2_s16* const in = static_cast<const vec2_s16*>(__builtin_assume_aligned(&src.p[i * decimation_factor], 4));
|
||||
|
||||
complex32_t accum;
|
||||
|
||||
// Oldest samples are discarded.
|
||||
accum = mac_shift(z, t, 0, accum);
|
||||
accum = mac_shift(z, t, 1, accum);
|
||||
accum = mac_shift(z, t, 2, accum);
|
||||
accum = mac_shift(z, t, 3, accum);
|
||||
|
||||
// Middle samples are shifted earlier in the "z" delay buffer.
|
||||
accum = mac_shift_and_store(z, t, decimation_factor, 0, accum);
|
||||
accum = mac_shift_and_store(z, t, decimation_factor, 1, accum);
|
||||
accum = mac_shift_and_store(z, t, decimation_factor, 2, accum);
|
||||
accum = mac_shift_and_store(z, t, decimation_factor, 3, accum);
|
||||
accum = mac_shift_and_store(z, t, decimation_factor, 4, accum);
|
||||
accum = mac_shift_and_store(z, t, decimation_factor, 5, accum);
|
||||
accum = mac_shift_and_store(z, t, decimation_factor, 6, accum);
|
||||
accum = mac_shift_and_store(z, t, decimation_factor, 7, accum);
|
||||
|
||||
// Newest samples come from "in" buffer, are copied to "z" delay buffer.
|
||||
accum = mac_shift_and_store_new_c16_samples(z, t, in, decimation_factor, 0, taps_count, accum);
|
||||
accum = mac_shift_and_store_new_c16_samples(z, t, in, decimation_factor, 1, taps_count, accum);
|
||||
accum = mac_shift_and_store_new_c16_samples(z, t, in, decimation_factor, 2, taps_count, accum);
|
||||
accum = mac_shift_and_store_new_c16_samples(z, t, in, decimation_factor, 3, taps_count, accum);
|
||||
|
||||
d[i] = scale_round_and_pack(accum, k);
|
||||
}
|
||||
|
||||
return {
|
||||
dst.p,
|
||||
count,
|
||||
src.sampling_rate / decimation_factor
|
||||
};
|
||||
}
|
||||
|
||||
buffer_c16_t Complex8DecimateBy2CIC3::execute(const buffer_c8_t& src, const buffer_c16_t& dst) {
|
||||
/* Decimates by two using a non-recursive third-order CIC filter.
|
||||
*/
|
||||
|
||||
/* CIC filter (decimating by two):
|
||||
* D_I0 = i3 * 1 + i2 * 3 + i1 * 3 + i0 * 1
|
||||
* D_Q0 = q3 * 1 + q2 * 3 + q1 * 3 + q0 * 1
|
||||
*
|
||||
* D_I1 = i5 * 1 + i4 * 3 + i3 * 3 + i2 * 1
|
||||
* D_Q1 = q5 * 1 + q4 * 3 + q3 * 3 + q2 * 1
|
||||
*/
|
||||
|
||||
uint32_t i1_i0 = _i1_i0;
|
||||
uint32_t q1_q0 = _q1_q0;
|
||||
|
||||
/* 3:1 Scaled by 32 to normalize output to +/-32768-ish. */
|
||||
constexpr uint32_t scale_factor = 32;
|
||||
constexpr uint32_t k_3_1 = 0x00030001 * scale_factor;
|
||||
uint32_t* src_p = reinterpret_cast<uint32_t*>(&src.p[0]);
|
||||
uint32_t* const src_end = reinterpret_cast<uint32_t*>(&src.p[src.count]);
|
||||
uint32_t* dst_p = reinterpret_cast<uint32_t*>(&dst.p[0]);
|
||||
while(src_p < src_end) {
|
||||
const uint32_t q3_i3_q2_i2 = *(src_p++); // 3
|
||||
const uint32_t q5_i5_q4_i4 = *(src_p++);
|
||||
|
||||
const uint32_t d_i0_partial = __SMUAD(k_3_1, i1_i0); // 1: = 3 * i1 + 1 * i0
|
||||
const uint32_t i3_i2 = __SXTB16(q3_i3_q2_i2, 0); // 1: (q3_i3_q2_i2 ror 0)[23:16]:(q3_i3_q2_i2 ror 0)[7:0]
|
||||
const uint32_t d_i0 = __SMLADX(k_3_1, i3_i2, d_i0_partial); // 1: + 3 * i2 + 1 * i3
|
||||
|
||||
const uint32_t d_q0_partial = __SMUAD(k_3_1, q1_q0); // 1: = 3 * q1 * 1 * q0
|
||||
const uint32_t q3_q2 = __SXTB16(q3_i3_q2_i2, 8); // 1: (q3_i3_q2_i2 ror 8)[23:16]:(q3_i3_q2_i2 ror 8)[7:0]
|
||||
const uint32_t d_q0 = __SMLADX(k_3_1, q3_q2, d_q0_partial); // 1: + 3 * q2 + 1 * q3
|
||||
|
||||
const uint32_t d_q0_i0 = __PKHBT(d_i0, d_q0, 16); // 1: (Rm<<16)[31:16]:Rn[15:0]
|
||||
|
||||
const uint32_t d_i1_partial = __SMUAD(k_3_1, i3_i2); // 1: = 3 * i3 + 1 * i2
|
||||
const uint32_t i5_i4 = __SXTB16(q5_i5_q4_i4, 0); // 1: (q5_i5_q4_i4 ror 0)[23:16]:(q5_i5_q4_i4 ror 0)[7:0]
|
||||
const uint32_t d_i1 = __SMLADX(k_3_1, i5_i4, d_i1_partial); // 1: + 1 * i5 + 3 * i4
|
||||
|
||||
const uint32_t d_q1_partial = __SMUAD(k_3_1, q3_q2); // 1: = 3 * q3 * 1 * q2
|
||||
const uint32_t q5_q4 = __SXTB16(q5_i5_q4_i4, 8); // 1: (q5_i5_q4_i4 ror 8)[23:16]:(q5_i5_q4_i4 ror 8)[7:0]
|
||||
const uint32_t d_q1 = __SMLADX(k_3_1, q5_q4, d_q1_partial); // 1: + 1 * q5 + 3 * q4
|
||||
|
||||
const uint32_t d_q1_i1 = __PKHBT(d_i1, d_q1, 16); // 1: (Rm<<16)[31:16]:Rn[15:0]
|
||||
|
||||
*(dst_p++) = d_q0_i0; // 3
|
||||
*(dst_p++) = d_q1_i1;
|
||||
|
||||
i1_i0 = i5_i4;
|
||||
q1_q0 = q5_q4;
|
||||
}
|
||||
_i1_i0 = i1_i0;
|
||||
_q1_q0 = q1_q0;
|
||||
|
||||
return { dst.p, src.count / 2, src.sampling_rate / 2 };
|
||||
}
|
||||
|
||||
buffer_c16_t TranslateByFSOver4AndDecimateBy2CIC3::execute(const buffer_c8_t& src, const buffer_c16_t& dst) {
|
||||
/* Translates incoming complex<int8_t> samples by -fs/4,
|
||||
* decimates by two using a non-recursive third-order CIC filter.
|
||||
*/
|
||||
@@ -111,8 +555,8 @@ buffer_c16_t TranslateByFSOver4AndDecimateBy2CIC3::execute(buffer_c8_t src, buff
|
||||
}
|
||||
|
||||
buffer_c16_t DecimateBy2CIC3::execute(
|
||||
buffer_c16_t src,
|
||||
buffer_c16_t dst
|
||||
const buffer_c16_t& src,
|
||||
const buffer_c16_t& dst
|
||||
) {
|
||||
/* Complex non-recursive 3rd-order CIC filter (taps 1,3,3,1).
|
||||
* Gain of 8.
|
||||
@@ -121,20 +565,18 @@ buffer_c16_t DecimateBy2CIC3::execute(
|
||||
*/
|
||||
uint32_t t1 = _iq0;
|
||||
uint32_t t2 = _iq1;
|
||||
uint32_t t3, t4;
|
||||
const uint32_t taps = 0x00000003;
|
||||
auto s = src.p;
|
||||
auto d = dst.p;
|
||||
const auto d_end = &dst.p[src.count / 2];
|
||||
uint32_t i, q;
|
||||
while(d < d_end) {
|
||||
i = __SXTH(t1, 0); /* 1: I0 */
|
||||
q = __SXTH(t1, 16); /* 1: Q0 */
|
||||
uint32_t i = __SXTH(t1, 0); /* 1: I0 */
|
||||
uint32_t q = __SXTH(t1, 16); /* 1: Q0 */
|
||||
i = __SMLABB(t2, taps, i); /* 1: I1*3 + I0 */
|
||||
q = __SMLATB(t2, taps, q); /* 1: Q1*3 + Q0 */
|
||||
|
||||
t3 = *__SIMD32(s)++; /* 3: Q2:I2 */
|
||||
t4 = *__SIMD32(s)++; /* Q3:I3 */
|
||||
const uint32_t t3 = *__SIMD32(s)++; /* 3: Q2:I2 */
|
||||
const uint32_t t4 = *__SIMD32(s)++; /* Q3:I3 */
|
||||
|
||||
i = __SMLABB(t3, taps, i); /* 1: I2*3 + I1*3 + I0 */
|
||||
q = __SMLATB(t3, taps, q); /* 1: Q2*3 + Q1*3 + Q0 */
|
||||
@@ -164,9 +606,15 @@ buffer_c16_t DecimateBy2CIC3::execute(
|
||||
return { dst.p, src.count / 2, src.sampling_rate / 2 };
|
||||
}
|
||||
|
||||
void FIR64AndDecimateBy2Real::configure(
|
||||
const std::array<int16_t, taps_count>& new_taps
|
||||
) {
|
||||
std::copy(new_taps.cbegin(), new_taps.cend(), taps.begin());
|
||||
}
|
||||
|
||||
buffer_s16_t FIR64AndDecimateBy2Real::execute(
|
||||
buffer_s16_t src,
|
||||
buffer_s16_t dst
|
||||
const buffer_s16_t& src,
|
||||
const buffer_s16_t& dst
|
||||
) {
|
||||
/* int16_t input (sample count "n" must be multiple of 4)
|
||||
* -> int16_t output, decimated by 2.
|
||||
@@ -197,9 +645,18 @@ buffer_s16_t FIR64AndDecimateBy2Real::execute(
|
||||
return { dst.p, src.count / 2, src.sampling_rate / 2 };
|
||||
}
|
||||
|
||||
void FIRAndDecimateComplex::configure_common(
|
||||
const size_t taps_count, const size_t decimation_factor
|
||||
) {
|
||||
samples_ = std::make_unique<samples_t>(taps_count);
|
||||
taps_reversed_ = std::make_unique<taps_t>(taps_count);
|
||||
taps_count_ = taps_count;
|
||||
decimation_factor_ = decimation_factor;
|
||||
}
|
||||
|
||||
buffer_c16_t FIRAndDecimateComplex::execute(
|
||||
buffer_c16_t src,
|
||||
buffer_c16_t dst
|
||||
const buffer_c16_t& src,
|
||||
const buffer_c16_t& dst
|
||||
) {
|
||||
/* int16_t input (sample count "n" must be multiple of decimation_factor)
|
||||
* -> int16_t output, decimated by decimation_factor.
|
||||
@@ -308,8 +765,8 @@ buffer_c16_t FIRAndDecimateComplex::execute(
|
||||
}
|
||||
|
||||
buffer_s16_t DecimateBy2CIC4Real::execute(
|
||||
buffer_s16_t src,
|
||||
buffer_s16_t dst
|
||||
const buffer_s16_t& src,
|
||||
const buffer_s16_t& dst
|
||||
) {
|
||||
auto src_p = src.p;
|
||||
auto dst_p = dst.p;
|
||||
@@ -328,76 +785,6 @@ buffer_s16_t DecimateBy2CIC4Real::execute(
|
||||
|
||||
return { dst.p, src.count / 2, src.sampling_rate / 2 };
|
||||
}
|
||||
#if 0
|
||||
buffer_c16_t DecimateBy2HBF5Complex::execute(
|
||||
buffer_c16_t const src,
|
||||
buffer_c16_t const dst
|
||||
) {
|
||||
auto src_p = src.p;
|
||||
auto dst_p = dst.p;
|
||||
int32_t n = src.count;
|
||||
for(; n>0; n-=2) {
|
||||
/* TODO: Probably a lot of room to optimize... */
|
||||
z[0] = z[2];
|
||||
//z[1] = z[3];
|
||||
z[2] = z[4];
|
||||
//z[3] = z[5];
|
||||
z[4] = z[6];
|
||||
z[5] = z[7];
|
||||
z[6] = z[8];
|
||||
z[7] = z[9];
|
||||
z[8] = z[10];
|
||||
z[9] = *(src_p++);
|
||||
z[10] = *(src_p++);
|
||||
|
||||
int32_t t_real { z[5].real * 256 };
|
||||
int32_t t_imag { z[5].imag * 256 };
|
||||
t_real += (z[ 0].real + z[10].real) * 3;
|
||||
t_imag += (z[ 0].imag + z[10].imag) * 3;
|
||||
t_real -= (z[ 2].real + z[ 8].real) * 25;
|
||||
t_imag -= (z[ 2].imag + z[ 8].imag) * 25;
|
||||
t_real += (z[ 4].real + z[ 6].real) * 150;
|
||||
t_imag += (z[ 4].imag + z[ 6].imag) * 150;
|
||||
*(dst_p++) = { t_real / 256, t_imag / 256 };
|
||||
}
|
||||
|
||||
return { dst.p, src.count / 2, src.sampling_rate / 2 };
|
||||
}
|
||||
|
||||
buffer_c16_t DecimateBy2HBF7Complex::execute(
|
||||
buffer_c16_t const src,
|
||||
buffer_c16_t const dst
|
||||
) {
|
||||
auto src_p = src.p;
|
||||
auto dst_p = dst.p;
|
||||
int32_t n = src.count;
|
||||
for(; n>0; n-=2) {
|
||||
/* TODO: Probably a lot of room to optimize... */
|
||||
z[0] = z[2];
|
||||
//z[1] = z[3];
|
||||
z[2] = z[4];
|
||||
//z[3] = z[5];
|
||||
z[4] = z[6];
|
||||
z[5] = z[7];
|
||||
z[6] = z[8];
|
||||
z[7] = z[9];
|
||||
z[8] = z[10];
|
||||
z[9] = *(src_p++);
|
||||
z[10] = *(src_p++);
|
||||
|
||||
int32_t t_real { z[5].real * 512 };
|
||||
int32_t t_imag { z[5].imag * 512 };
|
||||
t_real += (z[ 0].real + z[10].real) * 7;
|
||||
t_imag += (z[ 0].imag + z[10].imag) * 7;
|
||||
t_real -= (z[ 2].real + z[ 8].real) * 53;
|
||||
t_imag -= (z[ 2].imag + z[ 8].imag) * 53;
|
||||
t_real += (z[ 4].real + z[ 6].real) * 302;
|
||||
t_imag += (z[ 4].imag + z[ 6].imag) * 302;
|
||||
*(dst_p++) = { t_real / 512, t_imag / 512 };
|
||||
}
|
||||
|
||||
return { dst.p, src.count / 2, src.sampling_rate / 2 };
|
||||
}
|
||||
#endif
|
||||
} /* namespace decimate */
|
||||
} /* namespace dsp */
|
||||
|
||||
Reference in New Issue
Block a user