src/math/trig.c

/*
 * Copyright (c) 2016, Intel Corporation
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *   * Neither the name of the Intel Corporation nor the
 *     names of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Seppo Ingalsuo <seppo.ingalsuo@linux.intel.com>
 *         Liam Girdwood <liam.r.girdwood@linux.intel.com>
 *         Keyon Jie <yang.jie@linux.intel.com>
 */

#include <stdint.h>
#include <sof/audio/format.h>
#include <sof/math/trig.h>


#define SINE_C_Q20 341782638 /* 2*SINE_NQUART/pi in Q12.20 */
#define SINE_NQUART 512 /* Must be 2^N */
#define SINE_TABLE_SIZE (SINE_NQUART+1)

/* An 1/4 period of sine wave as Q1.31 */
const int32_t sine_table[SINE_TABLE_SIZE] = {
    0,
    6588387,
    13176712,
    19764913,
    26352928,
    32940695,
    39528151,
    46115236,
    52701887,
    59288042,
    65873638,
    72458615,
    79042909,
    85626460,
    92209205,
    98791081,
    105372028,
    111951983,
    118530885,
    125108670,
    131685278,
    138260647,
    144834714,
    151407418,
    157978697,
    164548489,
    171116732,
    177683365,
    184248325,
    190811551,
    197372981,
    203932553,
    210490206,
    217045877,
    223599506,
    230151030,
    236700388,
    243247517,
    249792358,
    256334847,
    262874923,
    269412525,
    275947592,
    282480061,
    289009871,
    295536961,
    302061269,
    308582734,
    315101294,
    321616889,
    328129457,
    334638936,
    341145265,
    347648383,
    354148229,
    360644742,
    367137860,
    373627523,
    380113669,
    386596237,
    393075166,
    399550396,
    406021864,
    412489512,
    418953276,
    425413098,
    431868915,
    438320667,
    444768293,
    451211734,
    457650927,
    464085813,
    470516330,
    476942419,
    483364019,
    489781069,
    496193509,
    502601279,
    509004318,
    515402566,
    521795963,
    528184448,
    534567963,
    540946445,
    547319836,
    553688076,
    560051103,
    566408860,
    572761285,
    579108319,
    585449903,
    591785976,
    598116478,
    604441351,
    610760535,
    617073970,
    623381597,
    629683357,
    635979190,
    642269036,
    648552837,
    654830534,
    661102068,
    667367379,
    673626408,
    679879097,
    686125386,
    692365218,
    698598533,
    704825272,
    711045377,
    717258790,
    723465451,
    729665303,
    735858287,
    742044345,
    748223418,
    754395449,
    760560379,
    766718151,
    772868706,
    779011986,
    785147934,
    791276492,
    797397602,
    803511207,
    809617248,
    815715670,
    821806413,
    827889421,
    833964637,
    840032003,
    846091463,
    852142959,
    858186434,
    864221832,
    870249095,
    876268167,
    882278991,
    888281511,
    894275670,
    900261412,
    906238681,
    912207419,
    918167571,
    924119082,
    930061894,
    935995952,
    941921200,
    947837582,
    953745043,
    959643527,
    965532978,
    971413341,
    977284561,
    983146583,
    988999351,
    994842809,
    1000676905,
    1006501581,
    1012316784,
    1018122458,
    1023918549,
    1029705003,
    1035481765,
    1041248781,
    1047005996,
    1052753356,
    1058490807,
    1064218296,
    1069935767,
    1075643168,
    1081340445,
    1087027543,
    1092704410,
    1098370992,
    1104027236,
    1109673088,
    1115308496,
    1120933406,
    1126547765,
    1132151521,
    1137744620,
    1143327011,
    1148898640,
    1154459455,
    1160009404,
    1165548435,
    1171076495,
    1176593532,
    1182099495,
    1187594332,
    1193077990,
    1198550419,
    1204011566,
    1209461381,
    1214899812,
    1220326808,
    1225742318,
    1231146290,
    1236538675,
    1241919421,
    1247288477,
    1252645793,
    1257991319,
    1263325005,
    1268646799,
    1273956652,
    1279254515,
    1284540337,
    1289814068,
    1295075658,
    1300325059,
    1305562221,
    1310787095,
    1315999631,
    1321199780,
    1326387493,
    1331562722,
    1336725418,
    1341875532,
    1347013016,
    1352137822,
    1357249900,
    1362349204,
    1367435684,
    1372509294,
    1377569985,
    1382617710,
    1387652421,
    1392674071,
    1397682613,
    1402677999,
    1407660183,
    1412629117,
    1417584755,
    1422527050,
    1427455956,
    1432371426,
    1437273414,
    1442161874,
    1447036759,
    1451898025,
    1456745625,
    1461579513,
    1466399644,
    1471205973,
    1475998455,
    1480777044,
    1485541695,
    1490292364,
    1495029005,
    1499751575,
    1504460029,
    1509154322,
    1513834410,
    1518500249,
    1523151796,
    1527789006,
    1532411836,
    1537020243,
    1541614182,
    1546193612,
    1550758488,
    1555308767,
    1559844407,
    1564365366,
    1568871600,
    1573363067,
    1577839726,
    1582301533,
    1586748446,
    1591180425,
    1595597427,
    1599999410,
    1604386334,
    1608758157,
    1613114837,
    1617456334,
    1621782607,
    1626093615,
    1630389318,
    1634669675,
    1638934646,
    1643184190,
    1647418268,
    1651636840,
    1655839867,
    1660027308,
    1664199124,
    1668355276,
    1672495724,
    1676620431,
    1680729357,
    1684822463,
    1688899710,
    1692961061,
    1697006478,
    1701035921,
    1705049354,
    1709046738,
    1713028036,
    1716993211,
    1720942224,
    1724875039,
    1728791619,
    1732691927,
    1736575926,
    1740443580,
    1744294852,
    1748129706,
    1751948106,
    1755750016,
    1759535401,
    1763304223,
    1767056449,
    1770792043,
    1774510970,
    1778213194,
    1781898680,
    1785567395,
    1789219304,
    1792854372,
    1796472564,
    1800073848,
    1803658188,
    1807225552,
    1810775906,
    1814309215,
    1817825448,
    1821324571,
    1824806551,
    1828271355,
    1831718951,
    1835149305,
    1838562387,
    1841958164,
    1845336603,
    1848697673,
    1852041343,
    1855367580,
    1858676354,
    1861967633,
    1865241387,
    1868497585,
    1871736195,
    1874957188,
    1878160534,
    1881346201,
    1884514160,
    1887664382,
    1890796836,
    1893911493,
    1897008324,
    1900087300,
    1903148391,
    1906191569,
    1909216806,
    1912224072,
    1915213339,
    1918184580,
    1921137766,
    1924072870,
    1926989863,
    1929888719,
    1932769410,
    1935631909,
    1938476189,
    1941302224,
    1944109986,
    1946899450,
    1949670588,
    1952423376,
    1955157787,
    1957873795,
    1960571374,
    1963250500,
    1965911147,
    1968553291,
    1971176905,
    1973781966,
    1976368449,
    1978936330,
    1981485584,
    1984016188,
    1986528117,
    1989021349,
    1991495859,
    1993951624,
    1996388621,
    1998806828,
    2001206221,
    2003586778,
    2005948477,
    2008291295,
    2010615209,
    2012920200,
    2015206244,
    2017473320,
    2019721407,
    2021950483,
    2024160528,
    2026351521,
    2028523441,
    2030676268,
    2032809981,
    2034924561,
    2037019987,
    2039096240,
    2041153301,
    2043191149,
    2045209766,
    2047209132,
    2049189230,
    2051150040,
    2053091543,
    2055013722,
    2056916559,
    2058800035,
    2060664132,
    2062508835,
    2064334123,
    2066139982,
    2067926393,
    2069693341,
    2071440807,
    2073168776,
    2074877232,
    2076566159,
    2078235539,
    2079885359,
    2081515602,
    2083126253,
    2084717297,
    2086288719,
    2087840504,
    2089372637,
    2090885104,
    2092377891,
    2093850984,
    2095304369,
    2096738031,
    2098151959,
    2099546138,
    2100920555,
    2102275198,
    2103610053,
    2104925108,
    2106220351,
    2107495769,
    2108751351,
    2109987084,
    2111202958,
    2112398959,
    2113575079,
    2114731304,
    2115867625,
    2116984030,
    2118080510,
    2119157053,
    2120213650,
    2121250291,
    2122266966,
    2123263665,
    2124240379,
    2125197099,
    2126133816,
    2127050521,
    2127947205,
    2128823861,
    2129680479,
    2130517051,
    2131333571,
    2132130029,
    2132906419,
    2133662733,
    2134398965,
    2135115106,
    2135811152,
    2136487094,
    2137142926,
    2137778643,
    2138394239,
    2138989707,
    2139565042,
    2140120239,
    2140655292,
    2141170196,
    2141664947,
    2142139540,
    2142593970,
    2143028233,
    2143442325,
    2143836243,
    2144209981,
    2144563538,
    2144896909,
    2145210091,
    2145503082,
    2145775879,
    2146028479,
    2146260880,
    2146473079,
    2146665075,
    2146836865,
    2146988449,
    2147119824,
    2147230990,
    2147321945,
    2147392689,
    2147443221,
    2147473541,
    2147483647
};

/* Sine lookup table read */
static inline int32_t sine_lookup(int idx) {
    int32_t s;
    int i1;

    i1 = idx & (2 * SINE_NQUART - 1);
    if (i1 > SINE_NQUART)
        i1 = 2 * SINE_NQUART - i1;

    if (idx > 2 * SINE_NQUART)
        s = -sine_table[i1];
    else
        s = sine_table[i1];

    return (s);
}

/* Compute fixed point sine with table lookup and interpolation */
int32_t sin_fixed(int32_t w) {
    int idx;
    int32_t frac;
    int32_t s0;
    int32_t s1;
    int32_t delta;
    int64_t sine;
    int64_t idx_tmp;

    /* Q4.28 x Q12.20 -> Q16.48 */
    idx_tmp = (int64_t) w * SINE_C_Q20;
    idx = (int) (idx_tmp >> 48); /* Shift to Q0 */
    idx_tmp = idx_tmp >> 17; /* Shift to Q16.31 */
    idx_tmp = idx_tmp - (idx << 31); /* Get fraction */
    frac = (int32_t) idx_tmp; /* Q1.31 */
    s0 = sine_lookup(idx); /* Q1.31 */
    s1 = sine_lookup(idx + 1); /* Q1.31 */
    delta = s1 - s0; /* Q1.31 */
    //sine = (int64_t) frac*delta; /* Q1.31 x Q1.31 -> Q2.62 */
    //sine = (sine >> 31) + s0; /* Q2.31 */
	/* All Q1.31 */
	sine = s0 + q_mults_32x32(frac, delta, Q_SHIFT_BITS_64(31, 31, 31));
    return (int32_t) sine;
}