chris@160: /**
chris@160:  * Copyright (c) 2014, 2015, Enzien Audio Ltd.
chris@160:  *
chris@160:  * Permission to use, copy, modify, and/or distribute this software for any
chris@160:  * purpose with or without fee is hereby granted, provided that the above
chris@160:  * copyright notice and this permission notice appear in all copies.
chris@160:  *
chris@160:  * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH
chris@160:  * REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
chris@160:  * AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT,
chris@160:  * INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
chris@160:  * LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
chris@160:  * OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
chris@160:  * PERFORMANCE OF THIS SOFTWARE.
chris@160:  */
chris@160: 
chris@160: #include "SignalPhasor.h"
chris@160: 
chris@160: // input phase is in the range of [0,1]. It is independent of o->phase.
chris@160: #if HV_SIMD_AVX
chris@160: static void sPhasor_updatePhase(SignalPhasor *o, float p) {
chris@160:   o->phase = _mm256_set_ps(
chris@160:       p+1.0f+7.0f*o->step.f2sc, p+1.0f+6.0f*o->step.f2sc,
chris@160:       p+1.0f+5.0f*o->step.f2sc, p+1.0f+4.0f*o->step.f2sc,
chris@160:       p+1.0f+3.0f*o->step.f2sc, p+1.0f+2.0f*o->step.f2sc,
chris@160:       p+1.0f+o->step.f2sc,      p+1.0f);
chris@160: 
chris@160:   // ensure that o->phase is still in range [1,2]
chris@160:   o->phase = _mm256_or_ps(_mm256_andnot_ps(
chris@160:       _mm256_set1_ps(-INFINITY), o->phase), _mm256_set1_ps(1.0f));
chris@160: #elif HV_SIMD_SSE
chris@160: static void sPhasor_updatePhase(SignalPhasor *o, hv_uint32_t p) {
chris@160:   o->phase = _mm_set_epi32(3*o->step.s+p, 2*o->step.s+p, o->step.s+p, p);
chris@160: #elif HV_SIMD_NEON
chris@160: static void sPhasor_updatePhase(SignalPhasor *o, hv_uint32_t p) {
chris@160:   o->phase = (uint32x4_t) {p, o->step.s+p, 2*o->step.s+p, 3*o->step.s+p};
chris@160: #else // HV_SIMD_NONE
chris@160: static void sPhasor_updatePhase(SignalPhasor *o, hv_uint32_t p) {
chris@160:   o->phase = p;
chris@160: #endif
chris@160: }
chris@160: 
chris@160: static void sPhasor_updateFrequency(SignalPhasor *o, float f, double r) {
chris@160: #if HV_SIMD_AVX
chris@160:   o->step.f2sc = (float) (f/r);
chris@160:   o->inc = _mm256_set1_ps((float) (8.0f*f/r));
chris@160:   sPhasor_updatePhase(o, o->phase[0]);
chris@160: #elif HV_SIMD_SSE
chris@160:   o->step.s = (hv_int32_t) (f*(4294967296.0/r));
chris@160:   o->inc = _mm_set1_epi32(4*o->step.s);
chris@160:   sPhasor_updatePhase(o, (hv_uint32_t) (o->phase[0] & 0xFFFFFFFFL));
chris@160: #elif HV_SIMD_NEON
chris@160:   o->step.s = (hv_int32_t) (f*(4294967296.0/r));
chris@160:   o->inc = vdupq_n_s32(4*o->step.s);
chris@160:   sPhasor_updatePhase(o, vgetq_lane_u32(o->phase, 0));
chris@160: #else // HV_SIMD_NONE
chris@160:   o->step.s = (hv_int32_t) (f*(4294967296.0/r));
chris@160:   o->inc = o->step.s;
chris@160:   // no need to update phase
chris@160: #endif
chris@160: }
chris@160: 
chris@160: hv_size_t sPhasor_init(SignalPhasor *o, double samplerate) {
chris@160: #if HV_SIMD_AVX
chris@160:   o->phase = _mm256_set1_ps(1.0f);
chris@160:   o->inc = _mm256_setzero_ps();
chris@160:   o->step.f2sc = (float) (1.0/samplerate);
chris@160: #elif HV_SIMD_SSE
chris@160:   o->phase = _mm_setzero_si128();
chris@160:   o->inc = _mm_setzero_si128();
chris@160:   o->step.f2sc = (float) (4294967296.0/samplerate);
chris@160: #elif HV_SIMD_NEON
chris@160:   o->phase = vdupq_n_u32(0);
chris@160:   o->inc = vdupq_n_s32(0);
chris@160:   o->step.f2sc = (float) (4294967296.0/samplerate);
chris@160: #else // HV_SIMD_NONE
chris@160:   o->phase = 0;
chris@160:   o->inc = 0;
chris@160:   o->step.f2sc = (float) (4294967296.0/samplerate);
chris@160: #endif
chris@160:   return 0;
chris@160: }
chris@160: 
chris@160: void sPhasor_onMessage(HvBase *_c, SignalPhasor *o, int letIn, const HvMessage *m) {
chris@160:   if (letIn == 1) {
chris@160:     if (msg_isFloat(m,0)) {
chris@160:       float phase = msg_getFloat(m,0);
chris@160:       while (phase < 0.0f) phase += 1.0f; // wrap phase to [0,1]
chris@160:       while (phase > 1.0f) phase -= 1.0f;
chris@160: #if HV_SIMD_AVX
chris@160:       sPhasor_updatePhase(o, phase);
chris@160: #else // HV_SIMD_SSE || HV_SIMD_NEON || HV_SIMD_NONE
chris@160:       sPhasor_updatePhase(o, (hv_int32_t) (phase * 4294967296.0));
chris@160: #endif
chris@160:     }
chris@160:   }
chris@160: }
chris@160: 
chris@160: hv_size_t sPhasor_k_init(SignalPhasor *o, float frequency, double samplerate) {
chris@160:   sPhasor_updateFrequency(o, frequency, samplerate);
chris@160:   sPhasor_updatePhase(o, 0);
chris@160:   return 0;
chris@160: }
chris@160: 
chris@160: void sPhasor_k_onMessage(HvBase *_c, SignalPhasor *o, int letIn, const HvMessage *m) {
chris@160:   if (msg_isFloat(m,0)) {
chris@160:     switch (letIn) {
chris@160:       case 0: sPhasor_updateFrequency(o, msg_getFloat(m,0), ctx_getSampleRate(_c)); break;
chris@160:       case 1: {
chris@160:         float phase = msg_getFloat(m,0);
chris@160:         while (phase < 0.0f) phase += 1.0f; // wrap phase to [0,1]
chris@160:         while (phase > 1.0f) phase -= 1.0f;
chris@160: #if HV_SIMD_AVX
chris@160:         sPhasor_updatePhase(o, phase);
chris@160: #else // HV_SIMD_SSE || HV_SIMD_NEON || HV_SIMD_NONE
chris@160:         sPhasor_updatePhase(o, (hv_uint32_t) (phase * 4294967296.0));
chris@160: #endif
chris@160:         break;
chris@160:       }
chris@160:       default: break;
chris@160:     }
chris@160:   }
chris@160: }