q15_sinpi.c

/*
 * Copyright (C) 2020 Ayan Shafqat <ayan.x.shafqat@gmail.com>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */

#include <stdint.h>

#if (__xtensa__ == 1) && defined(__GNUC__)
#define SSAT(x)     (xtensa_clamps15(x))
#else
#define SSAT(x)     MAX(MIN((x), 32767), -32768)
#endif /* __xtensa__ == 1 */

#define MIN(X,Y)    ((X) < (Y) ? (X) : (Y))
#define MAX(X,Y)    ((X) > (Y) ? (X) : (Y))
#define ABS(x)      (__builtin_abs(x))
#define HALF        (16384)
#define ONE         (32768)
#define ZERO        (0)
#define SMALLV      (8)

#if (__xtensa__ == 1) && defined(__GNUC__)
static inline int   xtensa_clamps15(int x);
#endif /* __xtensa__ == 1 */

static int32_t K_s5(int32_t p);

/**
 * @brief Implementation of sin(π x) function from [-1, +1]
 *
 * @param phase A value between -1.0, and 1.0 in Q1.15 format
 *
 * @return int16_t sin(π x)
 */
int16_t q15_sinpi(int16_t phase)
{
    int32_t px = (int32_t)phase;
    int32_t y = 0;

    if (ABS(px) <= SMALLV)
        y = px;
    else if (px >= HALF)
        y = K_s5(ONE - px);
    else if (px <= -HALF)
        y = K_s5(-px - ONE);
    else
        y = K_s5(px);

    y = SSAT(y);

    return y;
}

/**
 * @brief Implementation of cos(π x) function from [-1, +1]
 *
 * @param phase A value between -1.0, and 1.0 in Q1.15 format
 *
 * @return int16_t cos(π x)
 */
int16_t q15_cospi(int16_t phase)
{
    return q15_sinpi(HALF - phase);
}

#define QX (12)
#define PX (15 - QX)
#define FRMUL(x, y) (((x) * (y)) >> QX)

/**
 * @brief Taylor series approximation of sin(p) for p near zero
 *
 * @details The input and output of this function is Q15, but the
 *  internal computation is done in Q12.
 *
 * @param p A Q15 number [-0.5, 0.5]
 *
 * @return int32_t SIN(X) in Q15
 */
static int32_t K_s5(int32_t p)  // Q17.15
{
    const int16_t A1 = (+12868); //  π
    const int16_t A3 = (-21166); // -π^3/3!
    const int16_t A5 = (+10446); //  π^5/5!

    p = p >> PX;                 // Q4.12
    int32_t p2 = FRMUL(p, p);

    int32_t res = A5;
    res = FRMUL(res, p2) + A3;
    res = FRMUL(res, p2) + A1;
    res = FRMUL(res, p );
    res = res << PX;

    return res; // Q1.15
}

#if (__xtensa__ == 1)
static inline int xtensa_clamps15(int x)
{
    asm("clamps %0, %1, 15"
                        :"=r"(x)
                        :"r"(x)
                        : /* No clobber */ );
    return x;
}
#endif