diff --git a/dsp.c b/dsp.c index 2f5f97e..6e9f607 100644 --- a/dsp.c +++ b/dsp.c @@ -41,57 +41,48 @@ const int16_t sincos_tbl[48][2] = { {-24636, -21605 }, {-32698, -2143 }, {-27246, 18205 }, {-10533, 31029 } }; -int64_t acc_samp_s; -int64_t acc_samp_c; -int64_t acc_ref_s; -int64_t acc_ref_c; +int32_t acc_samp_s; +int32_t acc_samp_c; +int32_t acc_ref_s; +int32_t acc_ref_c; void dsp_process(int16_t *capture, size_t length) { + uint32_t *p = (uint32_t*)capture; + uint32_t len = length / 2; uint32_t i; -#if 1 - acc_samp_s = 0; - acc_samp_c = 0; - acc_ref_s = 0; - acc_ref_c = 0; - for (i = 0; i < length; i+=2) { - int32_t ref = capture[i+0]; - int32_t smp = capture[i+1]; + int32_t samp_s = 0; + int32_t samp_c = 0; + int32_t ref_s = 0; + int32_t ref_c = 0; + + for (i = 0; i < len; i++) { + uint32_t sr = *p++; + int16_t ref = sr & 0xffff; + int16_t smp = (sr>>16) & 0xffff; #ifdef ENABLED_DUMP ref_buf[i] = ref; samp_buf[i] = smp; #endif - int32_t s = ((int16_t *)sincos_tbl)[i+0]; - int32_t c = ((int16_t *)sincos_tbl)[i+1]; - acc_samp_s += (smp * s); - acc_samp_c += (smp * c); - acc_ref_s += (ref * s); - acc_ref_c += (ref * c); - } -#else - uint32_t len = length / 2; - int64_t samp_s = 0; - int64_t samp_c = 0; - int64_t ref_s = 0; - int64_t ref_c = 0; - // HI LO - int32_t *cos_sin = (int32_t *)sincos_tbl; - int32_t *ref_smp = (int32_t *)capture; - for (i = 0; i < len; i++) { - // - samp_s = __SMLALBB(*ref_smp, *cos_sin, samp_s); // samp_s+= smp * sin - samp_c = __SMLALBT(*ref_smp, *cos_sin, samp_c); // samp_c+= smp * cos - ref_s = __SMLALTB(*ref_smp, *cos_sin, ref_s); // ref_s += ref * sin - ref_c = __SMLALTT(*ref_smp, *cos_sin, ref_c); // ref_s += ref * cos - ref_smp++; - cos_sin++; + int32_t s = sincos_tbl[i][0]; + int32_t c = sincos_tbl[i][1]; + samp_s += smp * s / 16; + samp_c += smp * c / 16; + ref_s += ref * s / 16; + ref_c += ref * c / 16; +#if 0 + uint32_t sc = *(uint32_t)&sincos_tbl[i]; + samp_s = __SMLABB(sr, sc, samp_s); + samp_c = __SMLABT(sr, sc, samp_c); + ref_s = __SMLATB(sr, sc, ref_s); + ref_c = __SMLATT(sr, sc, ref_c); +#endif } acc_samp_s = samp_s; acc_samp_c = samp_c; - acc_ref_s = ref_s; - acc_ref_c = ref_c; -#endif + acc_ref_s = ref_s; + acc_ref_c = ref_c; } void @@ -99,12 +90,12 @@ calculate_gamma(float gamma[2]) { #if 1 // calculate reflection coeff. by samp divide by ref - double rs = acc_ref_s; - double rc = acc_ref_c; - double rr = rs * rs + rc * rc; + float rs = acc_ref_s; + float rc = acc_ref_c; + float rr = rs * rs + rc * rc; //rr = sqrtf(rr) * 1e8; - double ss = acc_samp_s; - double sc = acc_samp_c; + float ss = acc_samp_s; + float sc = acc_samp_c; gamma[0] = (sc * rc + ss * rs) / rr; gamma[1] = (ss * rc - sc * rs) / rr; #elif 0