alc/mixer/mixer_neon.cpp

   1 #include "config.h"
   2
   3 #include <arm_neon.h>
   4
   5 #include <limits>
   6
   7 #include "AL/al.h"
   8 #include "AL/alc.h"
   9 #include "alcmain.h"
  10 #include "alu.h"
  11 #include "hrtf.h"
  12 #include "defs.h"
  13 #include "bsinc_defs.h"
  14 #include "hrtfbase.h"
  15
  16 struct NEONTag;
  17 struct LerpTag;
  18 struct BSincTag;
  19 struct FastBSincTag;
  20
  21
  22 namespace {
  23
  24 #define FRAC_PHASE_BITDIFF (FRACTIONBITS - BSINC_PHASE_BITS)
  25 #define FRAC_PHASE_DIFFONE (1<<FRAC_PHASE_BITDIFF)
  26
  27 inline void ApplyCoeffs(float2 *RESTRICT Values, const uint_fast32_t IrSize,
  28     const HrirArray &Coeffs, const float left, const float right)
  29 {
  30     float32x4_t leftright4;
  31     {
  32         float32x2_t leftright2 = vdup_n_f32(0.0);
  33         leftright2 = vset_lane_f32(left, leftright2, 0);
  34         leftright2 = vset_lane_f32(right, leftright2, 1);
  35         leftright4 = vcombine_f32(leftright2, leftright2);
  36     }
  37
  38     ASSUME(IrSize >= MIN_IR_LENGTH);
  39     for(size_t c{0};c < IrSize;c += 2)
  40     {
  41         float32x4_t vals = vld1q_f32(&Values[c][0]);
  42         float32x4_t coefs = vld1q_f32(&Coeffs[c][0]);
  43
  44         vals = vmlaq_f32(vals, coefs, leftright4);
  45
  46         vst1q_f32(&Values[c][0], vals);
  47     }
  48 }
  49
  50 } // namespace
  51
  52 template<>
  53 const float *Resample_<LerpTag,NEONTag>(const InterpState*, const float *RESTRICT src, ALuint frac,
  54     ALuint increment, const al::span<float> dst)
  55 {
  56     const int32x4_t increment4 = vdupq_n_s32(static_cast<int>(increment*4));
  57     const float32x4_t fracOne4 = vdupq_n_f32(1.0f/FRACTIONONE);
  58     const int32x4_t fracMask4 = vdupq_n_s32(FRACTIONMASK);
  59     alignas(16) ALuint pos_[4], frac_[4];
  60     int32x4_t pos4, frac4;
  61
  62     InitPosArrays(frac, increment, frac_, pos_, 4);
  63     frac4 = vld1q_s32(reinterpret_cast<int*>(frac_));
  64     pos4 = vld1q_s32(reinterpret_cast<int*>(pos_));
  65
  66     auto dst_iter = dst.begin();
  67     for(size_t todo{dst.size()>>2};todo;--todo)
  68     {
  69         const int pos0{vgetq_lane_s32(pos4, 0)};
  70         const int pos1{vgetq_lane_s32(pos4, 1)};
  71         const int pos2{vgetq_lane_s32(pos4, 2)};
  72         const int pos3{vgetq_lane_s32(pos4, 3)};
  73         const float32x4_t val1{src[pos0], src[pos1], src[pos2], src[pos3]};
  74         const float32x4_t val2{src[pos0+1], src[pos1+1], src[pos2+1], src[pos3+1]};
  75
  76         /* val1 + (val2-val1)*mu */
  77         const float32x4_t r0{vsubq_f32(val2, val1)};
  78         const float32x4_t mu{vmulq_f32(vcvtq_f32_s32(frac4), fracOne4)};
  79         const float32x4_t out{vmlaq_f32(val1, mu, r0)};
  80
  81         vst1q_f32(dst_iter, out);
  82         dst_iter += 4;
  83
  84         frac4 = vaddq_s32(frac4, increment4);
  85         pos4 = vaddq_s32(pos4, vshrq_n_s32(frac4, FRACTIONBITS));
  86         frac4 = vandq_s32(frac4, fracMask4);
  87     }
  88
  89     if(size_t todo{dst.size()&3})
  90     {
  91         src += static_cast<ALuint>(vgetq_lane_s32(pos4, 0));
  92         frac = static_cast<ALuint>(vgetq_lane_s32(frac4, 0));
  93
  94         do {
  95             *(dst_iter++) = lerp(src[0], src[1], static_cast<float>(frac) * (1.0f/FRACTIONONE));
  96
  97             frac += increment;
  98             src  += frac>>FRACTIONBITS;
  99             frac &= FRACTIONMASK;
 100         } while(--todo);
 101     }
 102     return dst.data();
 103 }
 104
 105 template<>
 106 const float *Resample_<BSincTag,NEONTag>(const InterpState *state, const float *RESTRICT src,
 107     ALuint frac, ALuint increment, const al::span<float> dst)
 108 {
 109     const float *const filter{state->bsinc.filter};
 110     const float32x4_t sf4{vdupq_n_f32(state->bsinc.sf)};
 111     const size_t m{state->bsinc.m};
 112
 113     src -= state->bsinc.l;
 114     for(float &out_sample : dst)
 115     {
 116         // Calculate the phase index and factor.
 117         const ALuint pi{frac >> FRAC_PHASE_BITDIFF};
 118         const float pf{static_cast<float>(frac & (FRAC_PHASE_DIFFONE-1)) *
 119             (1.0f/FRAC_PHASE_DIFFONE)};
 120
 121         // Apply the scale and phase interpolated filter.
 122         float32x4_t r4{vdupq_n_f32(0.0f)};
 123         {
 124             const float32x4_t pf4{vdupq_n_f32(pf)};
 125             const float *fil{filter + m*pi*4};
 126             const float *phd{fil + m};
 127             const float *scd{phd + m};
 128             const float *spd{scd + m};
 129             size_t td{m >> 2};
 130             size_t j{0u};
 131
 132             do {
 133                 /* f = ((fil + sf*scd) + pf*(phd + sf*spd)) */
 134                 const float32x4_t f4 = vmlaq_f32(
 135                     vmlaq_f32(vld1q_f32(&fil[j]), sf4, vld1q_f32(&scd[j])),
 136                     pf4, vmlaq_f32(vld1q_f32(&phd[j]), sf4, vld1q_f32(&spd[j])));
 137                 /* r += f*src */
 138                 r4 = vmlaq_f32(r4, f4, vld1q_f32(&src[j]));
 139                 j += 4;
 140             } while(--td);
 141         }
 142         r4 = vaddq_f32(r4, vrev64q_f32(r4));
 143         out_sample = vget_lane_f32(vadd_f32(vget_low_f32(r4), vget_high_f32(r4)), 0);
 144
 145         frac += increment;
 146         src  += frac>>FRACTIONBITS;
 147         frac &= FRACTIONMASK;
 148     }
 149     return dst.data();
 150 }
 151
 152 template<>
 153 const float *Resample_<FastBSincTag,NEONTag>(const InterpState *state,
 154     const float *RESTRICT src, ALuint frac, ALuint increment, const al::span<float> dst)
 155 {
 156     const float *const filter{state->bsinc.filter};
 157     const size_t m{state->bsinc.m};
 158
 159     src -= state->bsinc.l;
 160     for(float &out_sample : dst)
 161     {
 162         // Calculate the phase index and factor.
 163         const ALuint pi{frac >> FRAC_PHASE_BITDIFF};
 164         const float pf{static_cast<float>(frac & (FRAC_PHASE_DIFFONE-1)) *
 165             (1.0f/FRAC_PHASE_DIFFONE)};
 166
 167         // Apply the phase interpolated filter.
 168         float32x4_t r4{vdupq_n_f32(0.0f)};
 169         {
 170             const float32x4_t pf4{vdupq_n_f32(pf)};
 171             const float *fil{filter + m*pi*4};
 172             const float *phd{fil + m};
 173             size_t td{m >> 2};
 174             size_t j{0u};
 175
 176             do {
 177                 /* f = fil + pf*phd */
 178                 const float32x4_t f4 = vmlaq_f32(vld1q_f32(&fil[j]), pf4, vld1q_f32(&phd[j]));
 179                 /* r += f*src */
 180                 r4 = vmlaq_f32(r4, f4, vld1q_f32(&src[j]));
 181                 j += 4;
 182             } while(--td);
 183         }
 184         r4 = vaddq_f32(r4, vrev64q_f32(r4));
 185         out_sample = vget_lane_f32(vadd_f32(vget_low_f32(r4), vget_high_f32(r4)), 0);
 186
 187         frac += increment;
 188         src  += frac>>FRACTIONBITS;
 189         frac &= FRACTIONMASK;
 190     }
 191     return dst.data();
 192 }
 193
 194
 195 template<>
 196 void MixHrtf_<NEONTag>(const float *InSamples, float2 *AccumSamples, const ALuint IrSize,
 197     const MixHrtfFilter *hrtfparams, const size_t BufferSize)
 198 { MixHrtfBase<ApplyCoeffs>(InSamples, AccumSamples, IrSize, hrtfparams, BufferSize); }
 199
 200 template<>
 201 void MixHrtfBlend_<NEONTag>(const float *InSamples, float2 *AccumSamples, const ALuint IrSize,
 202     const HrtfFilter *oldparams, const MixHrtfFilter *newparams, const size_t BufferSize)
 203 {
 204     MixHrtfBlendBase<ApplyCoeffs>(InSamples, AccumSamples, IrSize, oldparams, newparams,
 205         BufferSize);
 206 }
 207
 208 template<>
 209 void MixDirectHrtf_<NEONTag>(FloatBufferLine &LeftOut, FloatBufferLine &RightOut,
 210     const al::span<const FloatBufferLine> InSamples, float2 *AccumSamples, DirectHrtfState *State,
 211     const size_t BufferSize)
 212 { MixDirectHrtfBase<ApplyCoeffs>(LeftOut, RightOut, InSamples, AccumSamples, State, BufferSize); }
 213
 214
 215 template<>
 216 void Mix_<NEONTag>(const al::span<const float> InSamples, const al::span<FloatBufferLine> OutBuffer,
 217     float *CurrentGains, const float *TargetGains, const size_t Counter, const size_t OutPos)
 218 {
 219     const float delta{(Counter > 0) ? 1.0f / static_cast<float>(Counter) : 0.0f};
 220     const auto min_len = minz(Counter, InSamples.size());
 221     const auto aligned_len = minz((min_len+3) & ~size_t{3}, InSamples.size()) - min_len;
 222
 223     for(FloatBufferLine &output : OutBuffer)
 224     {
 225         float *RESTRICT dst{al::assume_aligned<16>(output.data()+OutPos)};
 226         float gain{*CurrentGains};
 227         const float step{(*TargetGains-gain) * delta};
 228
 229         size_t pos{0};
 230         if(!(std::fabs(step) > std::numeric_limits<float>::epsilon()))
 231             gain = *TargetGains;
 232         else
 233         {
 234             float step_count{0.0f};
 235             /* Mix with applying gain steps in aligned multiples of 4. */
 236             if(size_t todo{(min_len-pos) >> 2})
 237             {
 238                 const float32x4_t four4{vdupq_n_f32(4.0f)};
 239                 const float32x4_t step4{vdupq_n_f32(step)};
 240                 const float32x4_t gain4{vdupq_n_f32(gain)};
 241                 float32x4_t step_count4{vdupq_n_f32(0.0f)};
 242                 step_count4 = vsetq_lane_f32(1.0f, step_count4, 1);
 243                 step_count4 = vsetq_lane_f32(2.0f, step_count4, 2);
 244                 step_count4 = vsetq_lane_f32(3.0f, step_count4, 3);
 245
 246                 do {
 247                     const float32x4_t val4 = vld1q_f32(&InSamples[pos]);
 248                     float32x4_t dry4 = vld1q_f32(&dst[pos]);
 249                     dry4 = vmlaq_f32(dry4, val4, vmlaq_f32(gain4, step4, step_count4));
 250                     step_count4 = vaddq_f32(step_count4, four4);
 251                     vst1q_f32(&dst[pos], dry4);
 252                     pos += 4;
 253                 } while(--todo);
 254                 /* NOTE: step_count4 now represents the next four counts after
 255                  * the last four mixed samples, so the lowest element
 256                  * represents the next step count to apply.
 257                  */
 258                 step_count = vgetq_lane_f32(step_count4, 0);
 259             }
 260             /* Mix with applying left over gain steps that aren't aligned multiples of 4. */
 261             for(size_t leftover{min_len&3};leftover;++pos,--leftover)
 262             {
 263                 dst[pos] += InSamples[pos] * (gain + step*step_count);
 264                 step_count += 1.0f;
 265             }
 266             if(pos == Counter)
 267                 gain = *TargetGains;
 268             else
 269                 gain += step*step_count;
 270
 271             /* Mix until pos is aligned with 4 or the mix is done. */
 272             for(size_t leftover{aligned_len&3};leftover;++pos,--leftover)
 273                 dst[pos] += InSamples[pos] * gain;
 274         }
 275         *CurrentGains = gain;
 276         ++CurrentGains;
 277         ++TargetGains;
 278
 279         if(!(std::fabs(gain) > GAIN_SILENCE_THRESHOLD))
 280             continue;
 281         if(size_t todo{(InSamples.size()-pos) >> 2})
 282         {
 283             const float32x4_t gain4 = vdupq_n_f32(gain);
 284             do {
 285                 const float32x4_t val4 = vld1q_f32(&InSamples[pos]);
 286                 float32x4_t dry4 = vld1q_f32(&dst[pos]);
 287                 dry4 = vmlaq_f32(dry4, val4, gain4);
 288                 vst1q_f32(&dst[pos], dry4);
 289                 pos += 4;
 290             } while(--todo);
 291         }
 292         for(size_t leftover{(InSamples.size()-pos)&3};leftover;++pos,--leftover)
 293             dst[pos] += InSamples[pos] * gain;
 294     }
 295 }