File content as of revision 6:ed82052bd505:

#include "mbed.h"
#include "dsp.h"

extern Serial pc;


int16_t htstat_buf[HILBERT_TRANSFORM_FIRSTATE_LENGTH];
int16_t cap_buf[2][CAPTURE_LEN];

int16_t fir_state[FIRSTATE_LENGTH];
int16_t fir_buf[CAPTURE_LEN];

int16_t cic_buf[CICBUF_LEN];
int16_t dma_buf[2][DMA_DATALEN];

CICState cic;

void cic_interpolate_x10(CICState *cic, uint32_t *src, int src_len, uint32_t *dst)
{
    uint32_t p0 = cic->p0;
    uint32_t s0 = cic->s0;
    int i;
    int j = 0;
    for (i = 0; i < src_len; i++) {
        uint32_t s = src[i];
        uint32_t d0 = __QSUB16(s, p0);
        s0 = __QADD16(s0, d0);        
        uint32_t s1 = __QADD16(s0, d0);        
        uint32_t s2 = __QADD16(s1, d0);        
        uint32_t s3 = __QADD16(s2, d0);        
        uint32_t s4 = __QADD16(s3, d0);        
        dst[j  ] = s0 >> 2;
        dst[j+1] = s1 >> 2;
        dst[j+2] = s2 >> 2;
        dst[j+3] = s3 >> 2;
        dst[j+4] = s4 >> 2;
        j += 5;
        s0 = s4;
        s0 = __QADD16(s0, d0);        
        s1 = __QADD16(s0, d0);
        s2 = __QADD16(s1, d0);        
        s3 = __QADD16(s2, d0);        
        s4 = __QADD16(s3, d0);        
        dst[j  ] = s0 >> 2;
        dst[j+1] = s1 >> 2;
        dst[j+2] = s2 >> 2;
        dst[j+3] = s3 >> 2;
        dst[j+4] = s4 >> 2;
        j += 5;
        s0 = s4;
        p0 = s;
    }
    cic->s0 = s0;
    cic->p0 = p0;
}

const int16_t fir_coeff[8][16] = {
       { -19,   -6,  102,  -22, -357,  264, 1798, 2064,  630, -337, -119,  111,   15,  -24,    0,    0},
       { -13,  -20,   76,   51, -311,  -29, 1441, 2206, 1037, -231, -223,   96,   45,  -25,   -6,    0},
       {  -6,  -25,   45,   96, -223, -231, 1037, 2206, 1441,  -29, -311,   51,   76,  -20,  -13,    0},
       {   0,  -24,   15,  111, -119, -337,  630, 2064, 1798,  264, -357,  -22,  102,   -6,  -19,    0},
       {   0,  -19,   -6,  102,  -22, -357,  264, 1798, 2064,  630, -337, -119,  111,   15,  -24,    0},
       {   0,  -13,  -20,   76,   51, -311,  -29, 1441, 2206, 1037, -231, -223,   96,   45,  -25,   -6},
       {   0,   -6,  -25,   45,   96, -223, -231, 1037, 2206, 1441,  -29, -311,   51,   76,  -20,  -13},
       {   0,    0,  -24,   15,  111, -119, -337,  630, 2064, 1798,  264, -357,  -22,  102,   -6,  -19}
};

void fir_resample_x4(uint32_t *src_state, uint32_t *dst, int dst_len)
{
    uint32_t *src = src_state;
    int index = 0;
    int i;
    for (i = 0; i < dst_len; i++) {
        uint32_t *kp = (uint32_t*)fir_coeff[index];
        uint32_t acc_i = 0;
        uint32_t acc_q = 0;

        // 8cycle x 8       
#define CELL(n) do { \
        uint32_t k = kp[n]; \
        uint32_t p0 = src[n*2]; \
        uint32_t p1 = src[n*2+1]; \
        uint32_t i01 = __PKHTB(p1, p0, 16); \
        uint32_t q01 = __PKHBT(p0, p1, 16); \
        acc_i = __SMLAD(k, i01, acc_i); \
        acc_q = __SMLAD(k, q01, acc_q); \
} while(0)

        CELL(0);  CELL(1);  CELL(2);  CELL(3);
        CELL(4);  CELL(5);  CELL(6);  CELL(7);
        *dst++ = __PKHTB(__SSAT16(acc_i, 16), __SSAT16(acc_q, 16), 16);

        index++;
        if (index >= 8) {
            index = 0;
            src += 2;
        }
    }
    
    for (i = 0; i < FIRSTATE_LENGTH/2; i++)
        *src_state++ = *src++;
}

void
interpolate_test()
{
    int freq = 1000;
    int ampl = 3000;
    int rate = 48000;
    for (int i = 0; i < CAPTURE_LEN; i++){
        cap_buf[0][i*2] = sin(2*3.141592 * i * freq / rate) * ampl; // Lch
        cap_buf[0][i*2+1] = cos(2*3.141592 * i * freq / rate) * ampl; // Rch 
    }
#if 0
    for (int i = 0; i < 48; i++){
        pc.printf("%d, ", cap_buf[i*2]);
    }
    pc.printf("\n\r");
#endif
    fir_resample_x4((uint32_t *)&cap_buf[0], (uint32_t*)&cic_buf, CICBUF_LEN);
    cic_interpolate_x10(&cic, (uint32_t*)&cic_buf, CICBUF_LEN, (uint32_t*)dma_buf[0]);
    fir_resample_x4((uint32_t *)&cap_buf[0], (uint32_t*)&cic_buf, CICBUF_LEN);
    cic_interpolate_x10(&cic, (uint32_t*)&cic_buf, CICBUF_LEN, (uint32_t*)dma_buf[0]);
#if 0
    for (int i = 0; i < 48*25/4; i++){
        pc.printf("%d, ", cic_buf[i*2]);
    }
    pc.printf("\n\r");
#endif
#if 0
    for (int i = DMA_DATALEN/2-400; i < DMA_DATALEN/2; i++){
        pc.printf("%d, ", dma_buf[i*2]);
    }
    pc.printf("\n\r");
    for (int i = 0; i < 400; i++){
        pc.printf("%d, ", dma_buf[i*2]);
    }
    pc.printf("\n\r");
#endif
}

// hilbert transform by 127 taps fir
int16_t hilbert127_fir_coeff[32] = {
        20848,  6917,  4112,  2897,  2212,  1768,  1454,  1219,  1036,
          887,   764,   661,   572,   496,   429,   371,   319,   274,
          234,   198,   167,   140,   117,    97,    79,    65,    53,
           44,    36,    31,    28,    26
};

void
hilbert_transform(uint32_t *src, uint32_t *dst, int dst_len, int sign)
{
    int j;
    int len = sizeof htstat_buf / sizeof(uint32_t);
    src -= len;
    // 240 * (208 + 12) = 52800cycle = 0.62ms@84MHz
    for (j = 0; j < dst_len; j++) {
        int i;
        int32_t acc = 0;
#define OFFSET 64
        // 16 * (10 + 3) = 208 cycle
        for (i = 0; i < 32; i += 2) {
            // 10 cycle
            uint32_t c = *(uint32_t*)&hilbert127_fir_coeff[i];
            uint32_t a0 = src[OFFSET - i*2 - 1];
            uint32_t a1 = src[OFFSET - i*2 - 3];
            uint32_t b0 = src[OFFSET + i*2 + 1];
            uint32_t b1 = src[OFFSET + i*2 + 3];
            // fetch only R-ch (top half of word) from 2 successive samples
            uint32_t a = __PKHTB(a1, a0, 16);
            // and also do at symmetry samples
            uint32_t b = __PKHTB(b1, b0, 16);
            uint32_t d = __QSUB16(b, a);
            acc = __SMLAD(c, d, acc);
        }
        acc *= sign;
        int32_t real = src[OFFSET];
        real /= 2;
        *dst++ = __PKHTB(real, acc, 16);
        src++;
    }
}

void
hilbert_transform_save_fir_state(uint32_t *src_tail)
{
    int len = sizeof htstat_buf / sizeof(uint32_t);
    uint32_t *dst = (uint32_t*)&htstat_buf[0];
    uint32_t *src = src_tail - len;
    int i;
    for (i = 0; i < len; i++) {
        *dst++ = *src++;
    }
}

const int16_t cos_sin_table[257][4] = {
    { -32767,   10,      0, -804 },
    { -32757,   29,   -804, -804 },
    { -32728,   50,  -1608, -802 },
    { -32678,   69,  -2410, -802 },
    { -32609,   88,  -3212, -799 },
    { -32521,  109,  -4011, -797 },
    { -32412,  127,  -4808, -794 },
    { -32285,  148,  -5602, -791 },
    { -32137,  166,  -6393, -786 },
    { -31971,  186,  -7179, -783 },
    { -31785,  205,  -7962, -777 },
    { -31580,  224,  -8739, -773 },
    { -31356,  243,  -9512, -766 },
    { -31113,  261, -10278, -761 },
    { -30852,  281, -11039, -754 },
    { -30571,  298, -11793, -746 },
    { -30273,  317, -12539, -740 },
    { -29956,  335, -13279, -731 },
    { -29621,  353, -14010, -722 },
    { -29268,  370, -14732, -714 },
    { -28898,  388, -15446, -705 },
    { -28510,  405, -16151, -695 },
    { -28105,  422, -16846, -684 },
    { -27683,  438, -17530, -674 },
    { -27245,  455, -18204, -664 },
    { -26790,  471, -18868, -651 },
    { -26319,  487, -19519, -640 },
    { -25832,  503, -20159, -628 },
    { -25329,  518, -20787, -616 },
    { -24811,  532, -21403, -602 },
    { -24279,  548, -22005, -589 },
    { -23731,  561, -22594, -576 },
    { -23170,  576, -23170, -561 },
    { -22594,  589, -23731, -548 },
    { -22005,  602, -24279, -532 },
    { -21403,  616, -24811, -518 },
    { -20787,  628, -25329, -503 },
    { -20159,  640, -25832, -487 },
    { -19519,  651, -26319, -471 },
    { -18868,  664, -26790, -455 },
    { -18204,  674, -27245, -438 },
    { -17530,  684, -27683, -422 },
    { -16846,  695, -28105, -405 },
    { -16151,  705, -28510, -388 },
    { -15446,  714, -28898, -370 },
    { -14732,  722, -29268, -353 },
    { -14010,  731, -29621, -335 },
    { -13279,  740, -29956, -317 },
    { -12539,  746, -30273, -298 },
    { -11793,  754, -30571, -281 },
    { -11039,  761, -30852, -261 },
    { -10278,  766, -31113, -243 },
    { -9512,  773, -31356, -224 },
    { -8739,  777, -31580, -205 },
    { -7962,  783, -31785, -186 },
    { -7179,  786, -31971, -166 },
    { -6393,  791, -32137, -148 },
    { -5602,  794, -32285, -127 },
    { -4808,  797, -32412, -109 },
    { -4011,  799, -32521,  -88 },
    { -3212,  802, -32609,  -69 },
    { -2410,  802, -32678,  -50 },
    { -1608,  804, -32728,  -29 },
    {  -804,  804, -32757,  -10 },
    {     0,  804, -32767,   10 },
    {   804,  804, -32757,   29 },
    {  1608,  802, -32728,   50 },
    {  2410,  802, -32678,   69 },
    {  3212,  799, -32609,   88 },
    {  4011,  797, -32521,  109 },
    {  4808,  794, -32412,  127 },
    {  5602,  791, -32285,  148 },
    {  6393,  786, -32137,  166 },
    {  7179,  783, -31971,  186 },
    {  7962,  777, -31785,  205 },
    {  8739,  773, -31580,  224 },
    {  9512,  766, -31356,  243 },
    { 10278,  761, -31113,  261 },
    { 11039,  754, -30852,  281 },
    { 11793,  746, -30571,  298 },
    { 12539,  740, -30273,  317 },
    { 13279,  731, -29956,  335 },
    { 14010,  722, -29621,  353 },
    { 14732,  714, -29268,  370 },
    { 15446,  705, -28898,  388 },
    { 16151,  695, -28510,  405 },
    { 16846,  684, -28105,  422 },
    { 17530,  674, -27683,  438 },
    { 18204,  664, -27245,  455 },
    { 18868,  651, -26790,  471 },
    { 19519,  640, -26319,  487 },
    { 20159,  628, -25832,  503 },
    { 20787,  616, -25329,  518 },
    { 21403,  602, -24811,  532 },
    { 22005,  589, -24279,  548 },
    { 22594,  576, -23731,  561 },
    { 23170,  561, -23170,  576 },
    { 23731,  548, -22594,  589 },
    { 24279,  532, -22005,  602 },
    { 24811,  518, -21403,  616 },
    { 25329,  503, -20787,  628 },
    { 25832,  487, -20159,  640 },
    { 26319,  471, -19519,  651 },
    { 26790,  455, -18868,  664 },
    { 27245,  438, -18204,  674 },
    { 27683,  422, -17530,  684 },
    { 28105,  405, -16846,  695 },
    { 28510,  388, -16151,  705 },
    { 28898,  370, -15446,  714 },
    { 29268,  353, -14732,  722 },
    { 29621,  335, -14010,  731 },
    { 29956,  317, -13279,  740 },
    { 30273,  298, -12539,  746 },
    { 30571,  281, -11793,  754 },
    { 30852,  261, -11039,  761 },
    { 31113,  243, -10278,  766 },
    { 31356,  224,  -9512,  773 },
    { 31580,  205,  -8739,  777 },
    { 31785,  186,  -7962,  783 },
    { 31971,  166,  -7179,  786 },
    { 32137,  148,  -6393,  791 },
    { 32285,  127,  -5602,  794 },
    { 32412,  109,  -4808,  797 },
    { 32521,   88,  -4011,  799 },
    { 32609,   69,  -3212,  802 },
    { 32678,   50,  -2410,  802 },
    { 32728,   29,  -1608,  804 },
    { 32757,   10,   -804,  804 },
    { 32767,  -10,      0,  804 },
    { 32757,  -29,    804,  804 },
    { 32728,  -50,   1608,  802 },
    { 32678,  -69,   2410,  802 },
    { 32609,  -88,   3212,  799 },
    { 32521, -109,   4011,  797 },
    { 32412, -127,   4808,  794 },
    { 32285, -148,   5602,  791 },
    { 32137, -166,   6393,  786 },
    { 31971, -186,   7179,  783 },
    { 31785, -205,   7962,  777 },
    { 31580, -224,   8739,  773 },
    { 31356, -243,   9512,  766 },
    { 31113, -261,  10278,  761 },
    { 30852, -281,  11039,  754 },
    { 30571, -298,  11793,  746 },
    { 30273, -317,  12539,  740 },
    { 29956, -335,  13279,  731 },
    { 29621, -353,  14010,  722 },
    { 29268, -370,  14732,  714 },
    { 28898, -388,  15446,  705 },
    { 28510, -405,  16151,  695 },
    { 28105, -422,  16846,  684 },
    { 27683, -438,  17530,  674 },
    { 27245, -455,  18204,  664 },
    { 26790, -471,  18868,  651 },
    { 26319, -487,  19519,  640 },
    { 25832, -503,  20159,  628 },
    { 25329, -518,  20787,  616 },
    { 24811, -532,  21403,  602 },
    { 24279, -548,  22005,  589 },
    { 23731, -561,  22594,  576 },
    { 23170, -576,  23170,  561 },
    { 22594, -589,  23731,  548 },
    { 22005, -602,  24279,  532 },
    { 21403, -616,  24811,  518 },
    { 20787, -628,  25329,  503 },
    { 20159, -640,  25832,  487 },
    { 19519, -651,  26319,  471 },
    { 18868, -664,  26790,  455 },
    { 18204, -674,  27245,  438 },
    { 17530, -684,  27683,  422 },
    { 16846, -695,  28105,  405 },
    { 16151, -705,  28510,  388 },
    { 15446, -714,  28898,  370 },
    { 14732, -722,  29268,  353 },
    { 14010, -731,  29621,  335 },
    { 13279, -740,  29956,  317 },
    { 12539, -746,  30273,  298 },
    { 11793, -754,  30571,  281 },
    { 11039, -761,  30852,  261 },
    { 10278, -766,  31113,  243 },
    {  9512, -773,  31356,  224 },
    {  8739, -777,  31580,  205 },
    {  7962, -783,  31785,  186 },
    {  7179, -786,  31971,  166 },
    {  6393, -791,  32137,  148 },
    {  5602, -794,  32285,  127 },
    {  4808, -797,  32412,  109 },
    {  4011, -799,  32521,   88 },
    {  3212, -802,  32609,   69 },
    {  2410, -802,  32678,   50 },
    {  1608, -804,  32728,   29 },
    {   804, -804,  32757,   10 },
    {     0, -804,  32767,  -10 },
    {  -804, -804,  32757,  -29 },
    { -1608, -802,  32728,  -50 },
    { -2410, -802,  32678,  -69 },
    { -3212, -799,  32609,  -88 },
    { -4011, -797,  32521, -109 },
    { -4808, -794,  32412, -127 },
    { -5602, -791,  32285, -148 },
    { -6393, -786,  32137, -166 },
    { -7179, -783,  31971, -186 },
    { -7962, -777,  31785, -205 },
    { -8739, -773,  31580, -224 },
    { -9512, -766,  31356, -243 },
    { -10278, -761,  31113, -261 },
    { -11039, -754,  30852, -281 },
    { -11793, -746,  30571, -298 },
    { -12539, -740,  30273, -317 },
    { -13279, -731,  29956, -335 },
    { -14010, -722,  29621, -353 },
    { -14732, -714,  29268, -370 },
    { -15446, -705,  28898, -388 },
    { -16151, -695,  28510, -405 },
    { -16846, -684,  28105, -422 },
    { -17530, -674,  27683, -438 },
    { -18204, -664,  27245, -455 },
    { -18868, -651,  26790, -471 },
    { -19519, -640,  26319, -487 },
    { -20159, -628,  25832, -503 },
    { -20787, -616,  25329, -518 },
    { -21403, -602,  24811, -532 },
    { -22005, -589,  24279, -548 },
    { -22594, -576,  23731, -561 },
    { -23170, -561,  23170, -576 },
    { -23731, -548,  22594, -589 },
    { -24279, -532,  22005, -602 },
    { -24811, -518,  21403, -616 },
    { -25329, -503,  20787, -628 },
    { -25832, -487,  20159, -640 },
    { -26319, -471,  19519, -651 },
    { -26790, -455,  18868, -664 },
    { -27245, -438,  18204, -674 },
    { -27683, -422,  17530, -684 },
    { -28105, -405,  16846, -695 },
    { -28510, -388,  16151, -705 },
    { -28898, -370,  15446, -714 },
    { -29268, -353,  14732, -722 },
    { -29621, -335,  14010, -731 },
    { -29956, -317,  13279, -740 },
    { -30273, -298,  12539, -746 },
    { -30571, -281,  11793, -754 },
    { -30852, -261,  11039, -761 },
    { -31113, -243,  10278, -766 },
    { -31356, -224,   9512, -773 },
    { -31580, -205,   8739, -777 },
    { -31785, -186,   7962, -783 },
    { -31971, -166,   7179, -786 },
    { -32137, -148,   6393, -791 },
    { -32285, -127,   5602, -794 },
    { -32412, -109,   4808, -797 },
    { -32521,  -88,   4011, -799 },
    { -32609,  -69,   3212, -802 },
    { -32678,  -50,   2410, -802 },
    { -32728,  -29,   1608, -804 },
    { -32757,  -10,    804, -804 },
    { -32767,  -10,      0,    0 }
};

static inline 
uint32_t cos_sin(int16_t x)
{
    int idx = x / 256;
    int mod = x & 0xff;
    uint32_t r = __PKHBT(0x0100, mod, 16);
    uint32_t *e = (uint32_t *)&cos_sin_table[idx+128];
    uint32_t cd = e[0];
    uint32_t sd = e[1];
    int32_t c = __SMUAD(r, cd);
    int32_t s = __SMUAD(r, sd);
    c >>= 8;
    s >>= 8;
    return __PKHBT(s, c, 16);
}

FMModState fmmod;

void
fmmod_init(FMModState *fmmod)
{
    fmmod->phase = 0; 
}
    
void
frequency_modulation(FMModState *fmmod, uint32_t *src, uint32_t *dst, int dst_len)
{
    int j;
    uint16_t phase = fmmod->phase;
    for (j = 0; j < dst_len; j++) {
        uint32_t s = *src++;
        // fetch only R-ch (top half of word)
        int16_t x = s >> 16;
        phase += x;
        *dst++ = cos_sin(phase);
    }
    fmmod->phase = phase;
}

void
amplitude_modulation(uint32_t *src, uint32_t *dst, int dst_len)
{
    int j;
    for (j = 0; j < dst_len; j++) {
        uint32_t s = *src++;
        // fetch only R-ch (top half of word)
        int16_t x = s >> 16;
        // add DC and set zero at quadrature
        x = __SSAT(x + 0x3800, 16);
        *dst++ = __PKHBT(x, x, 16);
    }        
}