File content as of revision 6:ed82052bd505:

#include "mbed.h"
#include "pinmap.h"

#include "dsp.h"

I2C i2c(I2C_SDA, I2C_SCL);
DigitalOut myled(PA_10);
Serial pc(SERIAL_TX, SERIAL_RX);

SPI dds_spi(PA_7, NC, PA_5);
DigitalOut dds_reset(PA_0);
DigitalOut dds_dpd(PA_1);
DigitalOut dds_cs(PA_4);
DigitalOut dds_txen(PA_6);
DigitalIn dds_plllockdetect(PA_13);
DigitalIn dds_cicoverflow(PA_14);
PortOut data(PortC, 0x3fff);


void
dds_write(uint8_t reg, uint8_t value)
{
    dds_spi.write(reg);
    dds_spi.write(value);
}

void
dds_write4(uint8_t reg, uint32_t value)
{
    dds_spi.write(reg | 0x60);    
    dds_spi.write(value >> 24);
    dds_spi.write(value >> 16);
    dds_spi.write(value >> 8);
    dds_spi.write(value >> 0);
}

void
dds_init()
{
    dds_dpd = 0;
    dds_txen = 0;
    dds_cs = 1;
    dds_reset = 1;
    wait_us(10);
    dds_reset = 0;
    wait_us(200);
    dds_cs = 0;
    //dds_write(0x01, 0x01); // Single Tone Mode
    dds_write(0x01, 0x00); // Quadrature Mode
    //dds_write(0x01, 0x02); // DAC Mode
    //dds_write(0x00, 0x20 | 0x08); // REFCLK MULTI 8
    //dds_write(0x00, 0x20 | 0x14); // REFCLK MULTI 20
    dds_write(0x00, 0x20 | 16); // REFCLK MULTI 16 (12MHz x 16 = 192MHz)
    //dds_write(0x06, 0x20 << 2); // CIC interpolation ratio: 32
    dds_write(0x06, 25 << 2); // CIC interpolation ratio: 25
    dds_write(0x07, 0xff); // Scale Output: 0xff
    dds_cs = 1;
    while (dds_plllockdetect == 0)
        ;
}

void
dds_setup()
{
    dds_cs = 0;
    dds_txen = 0;
    //dds_write4(0x05, 11e6 * (65536.0*65536.0) / (12e6 * 16));
    dds_write4(0x05, 51.5e6 * (65536.0*65536.0) / (12e6 * 16));
    //dds_write4(0x05, 13.85e6 * (65536.0*65536.0) / (12e6 * 16));
    //dds_write4(0x05, 82.5e6 * (65536.0*65536.0) / (12e6 * 16));
    //dds_write4(0x05, 21.5e6 * (65536.0*65536.0) / (12e6 * 16));
    //dds_write4(0x05, 82.5e6 * (65536.0*65536.0) / (12e6 * 20));
    dds_cs = 1;
}

#if 0
void dma_transfer_complete(DMA_HandleTypeDef *hdma)
{
    //dds_txen = 1;
    myled = 0;
}

void dma_error(DMA_HandleTypeDef *hdma)
{
    //led = 0;
}
#endif

DMA_HandleTypeDef DMA_HandleType ={
    DMA2_Stream1,
    {
        DMA_CHANNEL_6,  // Request source is TIM_CH1
        DMA_MEMORY_TO_PERIPH,
        DMA_PINC_DISABLE,
        DMA_MINC_ENABLE,             
        DMA_PDATAALIGN_HALFWORD,
        DMA_MDATAALIGN_WORD,
        DMA_CIRCULAR,//DMA_PFCTRL,//
        DMA_PRIORITY_HIGH,
        DMA_FIFOMODE_DISABLE,//DMA_FIFOMODE_ENABLE,
        DMA_FIFO_THRESHOLD_HALFFULL,
        DMA_MBURST_SINGLE,
        DMA_PBURST_SINGLE
    },
    HAL_UNLOCKED,
    HAL_DMA_STATE_RESET,//HAL_DMA_STATE_READY
    NULL, // parent
    NULL, //dma_transfer_complete, // XferCpltCallback
    NULL, // XferHalfCpltCallback
    NULL, // XferM1CpltCallback
    NULL, //dma_error, // XferErrorCallback
    NULL  // ErrorCode
};

TIM_HandleTypeDef         htim;

#if 0
extern "C" static void
DMA_TIM_IRQHandler()
{    
    HAL_DMA_IRQHandler(&DMA_HandleType);
}
#endif

void dma_init(void)
{
    TIM_IC_InitTypeDef       icconf;
    GPIO_InitTypeDef gpioconf;   
    __DMA2_CLK_ENABLE();
    __TIM1_CLK_ENABLE();
    __GPIOA_CLK_ENABLE();

    //NVIC_SetVector(DMA2_Stream1_IRQn, (uint32_t)DMA_TIM_IRQHandler);
    //HAL_NVIC_EnableIRQ(DMA2_Stream1_IRQn);
    
    /* PA8 -> TIM1_CH1  */
    gpioconf.Pin = GPIO_PIN_8;
    gpioconf.Mode = GPIO_MODE_AF_OD;
    gpioconf.Pull = GPIO_NOPULL;
    gpioconf.Speed = GPIO_SPEED_HIGH;
    gpioconf.Alternate = GPIO_AF1_TIM1;
    HAL_GPIO_Init(GPIOA, &gpioconf);

    HAL_DMA_Init(&DMA_HandleType);
    
    htim.Instance = TIM1;
    htim.Init.Period        = 1;
    htim.Init.Prescaler     = 0;
    htim.Init.ClockDivision = 0;
    htim.Init.CounterMode   = TIM_COUNTERMODE_UP;
    HAL_TIM_IC_Init(&htim);
    
    //icconf.ICPolarity  = TIM_ICPOLARITY_FALLING;
    icconf.ICPolarity  = TIM_ICPOLARITY_RISING;
    icconf.ICSelection = TIM_ICSELECTION_DIRECTTI;
    icconf.ICPrescaler = TIM_ICPSC_DIV1;
    icconf.ICFilter    = 0;
    HAL_TIM_IC_ConfigChannel(&htim, &icconf, TIM_CHANNEL_1);
    __HAL_TIM_ENABLE_DMA(&htim, TIM_DMA_CC1);
    
    /* Start the TIM1 Channel1 */
    HAL_TIM_IC_Start(&htim, TIM_CHANNEL_1);
}

void ddsdma_start()
{
    dds_txen = 1;
    HAL_DMA_Start_IT(&DMA_HandleType, (uint32_t)dma_buf, (uint32_t)&GPIOC->ODR, DMA_DATALEN*2);
}


static void I2CWrite(int addr, char d0, char d1)
{
    char buf[] = { d0, d1 };
    i2c.write(addr<<1, buf, 2);
}

static int I2CRead(int addr, char d0)
{
    char buf[] = { d0 };
    i2c.write(addr<<1, buf, 1, true);
    i2c.read((addr<<1)+1, buf, 1);
    return (unsigned char)buf[0];
}

static void init_tlv320aic3204()
{
    I2CWrite(0x18, 0x00, 0x00); /* Initialize to Page 0 */
    I2CWrite(0x18, 0x01, 0x01); /* Initialize the device through software reset */
    I2CWrite(0x18, 0x04, 0x43); /* PLL Clock High, MCLK, PLL */
    /* 12.000MHz*7.1680 = 86.016MHz, 86.016MHz/(2*7*128) = 48kHz */
    I2CWrite(0x18, 0x05, 0x91); /* Power up PLL, P=1,R=1 */
    I2CWrite(0x18, 0x06, 0x07); /* J=7 */
    I2CWrite(0x18, 0x07, 6);    /* D=1680 = (6<<8) + 144 */
    I2CWrite(0x18, 0x08, 144);
    I2CWrite(0x18, 0x0b, 0x82); /* Power up the NDAC divider with value 2 */
    I2CWrite(0x18, 0x0c, 0x87); /* Power up the MDAC divider with value 7 */
    I2CWrite(0x18, 0x0d, 0x00); /* Program the OSR of DAC to 128 */
    I2CWrite(0x18, 0x0e, 0x80);
    I2CWrite(0x18, 0x3c, 0x08); /* Set the DAC Mode to PRB_P8 */
    I2CWrite(0x18, 0x1b, 0x0c); /* Set the BCLK,WCLK as output */    
    I2CWrite(0x18, 0x1e, 0x80 + 28); /* Enable the BCLKN divider with value 28 */
    I2CWrite(0x18, 0x25, 0xee); /* DAC power up */
    I2CWrite(0x18, 0x00, 0x01); /* Select Page 1 */
    I2CWrite(0x18, 0x01, 0x08); /* Disable Internal Crude AVdd in presence of external AVdd supply or before powering up internal AVdd LDO*/
    I2CWrite(0x18, 0x02, 0x01); /* Enable Master Analog Power Control */
    I2CWrite(0x18, 0x7b, 0x01); /* Set the REF charging time to 40ms */
    I2CWrite(0x18, 0x14, 0x25); /* HP soft stepping settings for optimal pop performance at power up Rpop used is 6k with N = 6 and soft step = 20usec. This should work with 47uF coupling capacitor. Can try N=5,6 or 7 time constants as well. Trade-off delay vs “pop” sound. */
//  I2CWrite(0x18, 0x0a, 0x00); /* Set the Input Common Mode to 0.9V and Output Common Mode for Headphone to Input Common Mode */
    I2CWrite(0x18, 0x0a, 0x33); /* Set the Input Common Mode to 0.9V and Output Common Mode for Headphone to 1.65V */
    I2CWrite(0x18, 0x0c, 0x08); /* Route Left DAC to HPL */
    I2CWrite(0x18, 0x0d, 0x08); /* Route Right DAC to HPR */
    I2CWrite(0x18, 0x03, 0x00); /* Set the DAC PTM mode to PTM_P3/4 */
    I2CWrite(0x18, 0x04, 0x00);
    I2CWrite(0x18, 0x10, 0x0a); /* Set the HPL gain to 0dB */
    I2CWrite(0x18, 0x11, 0x0a); /* Set the HPR gain to 0dB */
    I2CWrite(0x18, 0x09, 0x30); /* Power up HPL and HPR drivers */
    
    I2CWrite(0x18, 0x00, 0x00); /* Select Page 0 */
    I2CWrite(0x18, 0x12, 0x87); /* Power up the NADC divider with value 7 */
    I2CWrite(0x18, 0x13, 0x82); /* Power up the MADC divider with value 2 */
    I2CWrite(0x18, 0x14, 0x80); /* Program the OSR of ADC to 128 */
    I2CWrite(0x18, 0x3d, 0x01); /* Select ADC PRB_R1 */
    I2CWrite(0x18, 0x00, 0x01); /* Select Page 1 */
    I2CWrite(0x18, 0x3d, 0x00); /* Select ADC PTM_R4 */
    I2CWrite(0x18, 0x47, 0x32); /* Set MicPGA startup delay to 3.1ms */
    I2CWrite(0x18, 0x7b, 0x01); /* Set the REF charging time to 40ms */
    I2CWrite(0x18, 0x34, 0x80); /* Route IN1L to LEFT_P with 20K input impedance */
    I2CWrite(0x18, 0x36, 0x80); /* Route CM and IN3R to LEFT_N with 20K */
    I2CWrite(0x18, 0x37, 0x80); /* Route IN1R and IN3R to RIGHT_P with input impedance of 20K */
    I2CWrite(0x18, 0x39, 0x80); /* Route CM to RIGHT_N with impedance of 20K */
    I2CWrite(0x18, 0x3b, 0x0); /* Unmute Left MICPGA, Gain selection of 32dB to make channel gain 0dB */
    I2CWrite(0x18, 0x3c, 0x0); /* Unmute Right MICPGA, Gain selection of 32dB to make channel gain 0dB */
    I2CWrite(0x18, 0x33, 0x60); /* Enable MIC bias, 2.5V */

    wait_ms(40);
    I2CWrite(0x18, 0x00, 0x00); /* Select Page 0 */
    I2CWrite(0x18, 0x3f, 0xd6); /* Power up the Left and Right DAC Channels with route the Left Audio digital data to Left Channel DAC and Right Audio digital data to Right Channel DAC */
    I2CWrite(0x18, 0x40, 0x00); /* Unmute the DAC digital volume control */
    I2CWrite(0x18, 0x51, 0xc0); /* Power up Left and Right ADC Channels */
    I2CWrite(0x18, 0x52, 0x00); /* Unmute Left and Right ADC Digital Volume Control */    
    
    I2CWrite(0x18, 0x43, 0x93); /* Enable Headphone detection, Debounce 256ms, Button Debounce 32ms */    
}

void route_dac_headset()
{
    I2CWrite(0x18, 0x00, 0x01); /* Select Page 1 */
    I2CWrite(0x18, 0x34, 0x00); /* Route IN1L to LEFT_P with 20K input impedance */
    I2CWrite(0x18, 0x36, 0x08); /* Route IN3R to LEFT_N with 20K */
    I2CWrite(0x18, 0x37, 0x08); /* Route IN3R to RIGHT_P with input impedance of 20K */
    I2CWrite(0x18, 0x39, 0x80); /* Route CM to RIGHT_N with impedance of 20K */
    I2CWrite(0x18, 0x3b, 0x50); /* Unmute Left MICPGA, Gain selection of 40dB to make channel gain 0dB */
    I2CWrite(0x18, 0x3c, 0x50); /* Unmute Right MICPGA, Gain selection of 40dB to make channel gain 0dB */
    I2CWrite(0x18, 0x00, 0x00); /* Select Page 0 */
}

void route_dac_linein()
{
    I2CWrite(0x18, 0x00, 0x01); /* Select Page 1 */
    I2CWrite(0x18, 0x34, 0x80); /* Route IN1L to LEFT_P with 20K input impedance */
    I2CWrite(0x18, 0x36, 0x80); /* Route CM and IN3R to LEFT_N with 20K */
    I2CWrite(0x18, 0x37, 0x80); /* Route IN1R and IN3R to RIGHT_P with input impedance of 20K */
    I2CWrite(0x18, 0x39, 0x80); /* Route CM to RIGHT_N with impedance of 20K */
    I2CWrite(0x18, 0x3b, 0x0); /* Unmute Left MICPGA, Gain selection of 32dB to make channel gain 0dB */
    I2CWrite(0x18, 0x3c, 0x0); /* Unmute Right MICPGA, Gain selection of 32dB to make channel gain 0dB */
    I2CWrite(0x18, 0x00, 0x00); /* Select Page 0 */
}

static const PinMap PinMap_I2S_CK[] = {
    {PB_10,  SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)},
    {PB_13,  SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)},
    {NC,    NC,    0}};
static const PinMap PinMap_I2S_WS[] = {
    {PB_12,  SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)},
    {PB_9,  SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)},
    {NC,    NC,    0}};
static const PinMap PinMap_I2S_SD[] = {
    {PC_3,  SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)},
    {PB_15,  SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)},
    {NC,    NC,    0}};
static const PinMap PinMap_I2Sext_SD[] = {
    {PB_14,  SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_I2S2ext)},
    {NC,    NC,    0}};

extern I2S_HandleTypeDef hi2s;

DMA_HandleTypeDef hdmatx ={
    DMA1_Stream4,
    { /*DMA_InitTypeDef*/
        DMA_CHANNEL_0,
        DMA_MEMORY_TO_PERIPH,
        DMA_PINC_DISABLE,
        DMA_MINC_ENABLE,             
        DMA_PDATAALIGN_HALFWORD,
        DMA_MDATAALIGN_WORD,
        DMA_CIRCULAR,//DMA_PFCTRL,
        DMA_PRIORITY_HIGH,
        DMA_FIFOMODE_DISABLE,//DMA_FIFOMODE_ENABLE
        DMA_FIFO_THRESHOLD_HALFFULL,
        DMA_MBURST_SINGLE,
        DMA_PBURST_SINGLE
    },
    HAL_UNLOCKED,
    HAL_DMA_STATE_RESET,//HAL_DMA_STATE_READY
    &hi2s, //Parent
    NULL,
    NULL,
    NULL,
    NULL,
    NULL    
};

DMA_HandleTypeDef hdmarx ={
    DMA1_Stream3,
    { /*DMA_InitTypeDef*/
        DMA_CHANNEL_3,
        DMA_PERIPH_TO_MEMORY,
        DMA_PINC_DISABLE,
        DMA_MINC_ENABLE,             
        DMA_PDATAALIGN_HALFWORD,
        DMA_MDATAALIGN_WORD,
        DMA_CIRCULAR,//DMA_PFCTRL,
        DMA_PRIORITY_HIGH,
        DMA_FIFOMODE_ENABLE,//DMA_FIFOMODE_DISABLE,
        DMA_FIFO_THRESHOLD_HALFFULL,
        DMA_MBURST_SINGLE,
        DMA_PBURST_SINGLE
    },
    HAL_UNLOCKED,
    HAL_DMA_STATE_RESET,//HAL_DMA_STATE_READY
    &hi2s, //Parent
    NULL,
    NULL,
    NULL,
    NULL,
    NULL    
};

I2S_HandleTypeDef hi2s = {
     SPI2,
     { /*I2S_InitTypeDef*/
        I2S_MODE_SLAVE_TX,
        I2S_STANDARD_PHILLIPS,
        I2S_DATAFORMAT_16B,
        I2S_MCLKOUTPUT_DISABLE,
        I2S_AUDIOFREQ_48K,
        I2S_CPOL_LOW,
        I2S_CLOCK_EXTERNAL,
        I2S_FULLDUPLEXMODE_ENABLE
     },
     NULL,  
     0,
     NULL,
     NULL,
     NULL,
     NULL,
     &hdmatx,
     &hdmarx,
     HAL_UNLOCKED,
     HAL_I2S_STATE_RESET,
     HAL_I2S_ERROR_NONE
};

extern "C" static void
DMATX_IRQHandler()
{    
    HAL_DMA_IRQHandler(&hdmatx);
}

extern "C" static void
DMARX_IRQHandler()
{    
    HAL_DMA_IRQHandler(&hdmarx);
}

void HAL_I2S_MspInit(I2S_HandleTypeDef *hi2s)
{
    NVIC_SetVector(DMA1_Stream4_IRQn, (uint32_t)DMATX_IRQHandler);
    NVIC_SetVector(DMA1_Stream3_IRQn, (uint32_t)DMARX_IRQHandler);
    HAL_NVIC_EnableIRQ(DMA1_Stream4_IRQn);
    HAL_NVIC_EnableIRQ(DMA1_Stream3_IRQn);
    //HAL_NVIC_SetPriority(DMA1_Stream4_IRQn, );
}

void
i2s_init()
{
    __SPI2_CLK_ENABLE();
    __DMA1_CLK_ENABLE();
    pinmap_pinout(PB_12, PinMap_I2S_WS);
    pinmap_pinout(PB_13, PinMap_I2S_CK);
    pinmap_pinout(PB_14, PinMap_I2Sext_SD);
    pinmap_pinout(PB_15, PinMap_I2S_SD);
    pin_mode(PB_12, PullUp);
    pin_mode(PB_13, PullUp);
    pin_mode(PB_14, PullUp);
    pin_mode(PB_15, PullUp);
    if (HAL_DMA_Init(&hdmatx) != HAL_OK)
        pc.printf("Error:HAL_DMA_Init\n\r");
    if (HAL_DMA_Init(&hdmarx) != HAL_OK)
        pc.printf("Error:HAL_DMA_Init\n\r");
    if (HAL_I2S_Init(&hi2s) != HAL_OK)
        pc.printf("Error:HAL_I2S_Init\n\r");
}

void i2sdma_start() 
{
    if (HAL_I2SEx_TransmitReceive_DMA(&hi2s, (uint16_t*)cap_buf, (uint16_t*)cap_buf, sizeof(cap_buf)/sizeof(uint16_t)) != HAL_OK)
        pc.printf("Error:HAL_I2S_Transmit_DMA\n\r");
}

static int first = true;

uint32_t capbuf_average(int buf)
{
    int32_t acc_i = 0;
    int32_t acc_q = 0;
    for (int i = 0; i < CAPTURE_LEN; i += 2){
        acc_i += cap_buf[buf][i+1];
        acc_q += cap_buf[buf][i];
    }
    acc_i /= CAPTURE_LEN/2;
    acc_q /= CAPTURE_LEN/2;
    return __PKHBT(acc_i, acc_q, 16);
}

uint32_t capbuf_ave;
uint32_t decay_ave;

void decay_average(uint32_t in)
{
    uint32_t i = __PKHBT(decay_ave, in, 16);
    uint32_t q = __PKHTB(in, decay_ave, 16);
    int32_t sum_i = __SMUAD(0x0001000f, i) / 16;
    int32_t sum_q = __SMUAD(0x0001000f, q) / 16;
    decay_ave = __PKHBT(sum_i, sum_q, 16);
}


enum {
    EVT_NONE = 0,
    EVT_BUTTON_PRESSED,
    EVT_HEADPHONE_PLUGGED,
    EVT_HEADSET_PLUGGED,
    EVT_UNPLUGGED
};

int
process_event()
{
    int stat = I2CRead(0x18, 67) & 0x60;
    int flag = I2CRead(0x18, 44);
    char *msg = NULL;
    int evt = 0;
    if (flag & 0x10) {
        if (stat == 0x60) {
            msg = "Headset plugged";
            evt = EVT_HEADSET_PLUGGED;
        } else if (stat == 0x20) {
            msg = "Headphone plugged";
            evt = EVT_HEADPHONE_PLUGGED;
        } else if (stat == 0x00) {
            msg = "Unplugged";
            evt = EVT_UNPLUGGED;
        }
    } else if (flag & 0x20) {
        msg = "Button pressed";
        evt = EVT_BUTTON_PRESSED;
    }
    if (msg)
        pc.printf("%s\r\n", msg);
    return evt;
}

enum {
    MODE_NFM = 0,
    MODE_AM,
    MODE_USB,
    MODE_LSB,
    MODE_IQ,
    MODE_NUM
};

int mode = MODE_NFM;//MODE_USB;

const char *mode_names[] = {
    "NFM", "AM", "USB", "LSB", "IQ"
};


void hilbert_transform_test();
void pseudo_noise_test();


int main()
{ 
    pc.baud(9600);
    pc.printf("AD9857 DDS IQ Synthesizer v0.0\r\n");
    pc.printf("SystemCoreClock: %dHz\r\n", SystemCoreClock);
    pc.printf("%s\r\n", mode_names[mode]);        
    //i2c.frequency(20000);
    // pullup I2C bus     
    pin_mode(PB_8, PullUp);
    pin_mode(PB_9, PullUp);
    
    dma_init();
    dds_init();
    dds_setup();

    fmmod_init(&fmmod);
    
    cic.s0 = __PKHBT(12, 0, 16); // adjust offset
    //pseudo_noise_test();
    //interpolate_test();
    //hilbert_transform_test();
    //ddsdma_start();   

    first = true;
    init_tlv320aic3204();
    i2s_init();
    i2sdma_start();

    while (1) {
        //pc.printf("%08x\r\n", fmmod.vec);
        //myled = !myled;
        wait(1);
        int evt = process_event();
        switch(evt) {
        case EVT_BUTTON_PRESSED:
            mode = (mode + 1) % MODE_NUM;
            pc.printf("%s\r\n", mode_names[mode]);        
            break;    
        case EVT_HEADSET_PLUGGED:
            route_dac_headset();
            break;
        case EVT_UNPLUGGED:
        case EVT_HEADPHONE_PLUGGED:
            route_dac_linein();
            break;    
        }

        //pc.printf("%08x %08x\r\n", decay_ave, capbuf_ave);
        //pc.printf("%02x %02x\r\n", I2CRead(0x18, 67), I2CRead(0x18, 44));
        if (pc.readable()) {
            pc.getc();
            uint32_t *buf = (uint32_t *)dma_buf;
            pc.printf("%08x %08x %08x %08x\r\n", buf[0], buf[1], buf[2], buf[3]);
        }
        //pc.printf("%04x %04x\r\n", (uint16_t)fir_buf[0], (uint16_t)fir_buf[1]);
        //pc.printf("%08x\r\n", *(uint32_t*)cap_buf[0]);
    } 
}

#if 0
void HAL_I2S_TxCpltCallback(I2S_HandleTypeDef *hi2s)
{
    //myled = 0;
}

void HAL_I2S_TxHalfCpltCallback(I2S_HandleTypeDef *hi2s)
{
    //myled = 1;
}
#endif

void offset_capture_buffer(uint32_t offset, uint32_t *dst, int dst_len)
{
    int i;
    for (i = 0; i < dst_len; i += 8) {
        dst[0] = __QSUB16(dst[0], offset);
        dst[1] = __QSUB16(dst[1], offset);
        dst[2] = __QSUB16(dst[2], offset);
        dst[3] = __QSUB16(dst[3], offset);
        dst[4] = __QSUB16(dst[4], offset);
        dst[5] = __QSUB16(dst[5], offset);
        dst[6] = __QSUB16(dst[6], offset);
        dst[7] = __QSUB16(dst[7], offset);
        dst += 8;
    }
}

static void
mod_fm_func(int buf)
{
    frequency_modulation(&fmmod, (uint32_t*)cap_buf[buf], (uint32_t*)fir_buf, sizeof fir_buf / sizeof(uint32_t));
}
static void
mod_am_func(int buf)
{
    amplitude_modulation((uint32_t*)cap_buf[buf], (uint32_t*)fir_buf, sizeof fir_buf / sizeof(uint32_t));
}

static void
mod_usb_func(int buf)
{
    hilbert_transform((uint32_t*)cap_buf[buf], (uint32_t*)fir_buf, sizeof fir_buf / sizeof(uint32_t), 1);
    if (buf)
        hilbert_transform_save_fir_state((uint32_t*)cap_buf[2]);    
}

static void
mod_lsb_func(int buf)
{
    hilbert_transform((uint32_t*)cap_buf[buf], (uint32_t*)fir_buf, sizeof fir_buf / sizeof(uint32_t), -1);
    if (buf)
        hilbert_transform_save_fir_state((uint32_t*)cap_buf[2]);    
}

static void
mod_raw_func(int buf)
{
    memcpy(fir_buf, cap_buf[buf], sizeof fir_buf);
}

void (*mod_funcs[])(int) = {
    mod_fm_func,
    mod_am_func,
    mod_usb_func,
    mod_lsb_func,
    mod_raw_func
};

void modulate_buffer_half(int buf)
{
    myled = 1;
    offset_capture_buffer(decay_ave, (uint32_t*)cap_buf[buf], sizeof cap_buf[0] / sizeof(uint32_t));
    //offset_capture_buffer(0x00780088, (uint32_t*)cap_buf[buf], sizeof cap_buf[0] / sizeof(uint32_t));
    myled = 0;
    myled = 1;
    (*mod_funcs[mode])(buf);
    myled = 0;
    myled = 1;
    int cic_len = sizeof(cic_buf)/sizeof(uint32_t);
    fir_resample_x4((uint32_t*)fir_state, (uint32_t*)cic_buf, cic_len);
    myled = 0;
    myled = 1;
    cic_interpolate_x10(&cic, (uint32_t*)cic_buf, cic_len, (uint32_t*)dma_buf[buf]);
    myled = 0;
}

void HAL_I2S_RxHalfCpltCallback(I2S_HandleTypeDef *hi2s)
{
    capbuf_ave = capbuf_average(0);
    decay_average(capbuf_ave);
    modulate_buffer_half(0);
    if (first) {
        ddsdma_start();
        first = false;
    }
}

void HAL_I2S_RxCpltCallback(I2S_HandleTypeDef *hi2s)
{
    capbuf_ave = capbuf_average(1);
    decay_average(capbuf_ave);
    modulate_buffer_half(1);
}

void
pseudo_noise_test()
{
#define RAND(N)     ((((float)rand() / RAND_MAX) * 2 * N) - N)
    int ampl = 10000;
    for (int i = 0; i < CAPTURE_LEN/2; i++){
        cap_buf[0][i*2] = RAND(ampl);
        cap_buf[0][i*2+1] = RAND(ampl); // Rch
        cap_buf[1][i*2] = RAND(ampl); // Lch
        cap_buf[1][i*2+1] = RAND(ampl); // Rch
    }
    modulate_buffer_half(0);
    modulate_buffer_half(1);
    modulate_buffer_half(0);
    modulate_buffer_half(1);
    ddsdma_start();
    pc.printf("Pseudo noise test\r\n");
    while(1);
    /* no reach */
}

void
hilbert_transform_test()
{
#if 1
    int freq = 800;
    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] = 0;//sin(2*3.141592 * i * freq / rate) * ampl; // Rch 
    }
#endif
    int cic_len = sizeof(cic_buf)/sizeof(uint32_t);
    hilbert_transform_save_fir_state((uint32_t*)cap_buf[1]);
    hilbert_transform((uint32_t*)cap_buf[0], (uint32_t*)fir_buf, sizeof fir_buf / sizeof(uint32_t), 1);
    hilbert_transform_save_fir_state((uint32_t*)cap_buf[1]);
    fir_resample_x4((uint32_t*)fir_state, (uint32_t*)cic_buf, cic_len);
    cic_interpolate_x10(&cic, (uint32_t*)cic_buf, cic_len, (uint32_t*)dma_buf[0]);
    
    hilbert_transform((uint32_t*)cap_buf[0], (uint32_t*)fir_buf, sizeof fir_buf / sizeof(uint32_t), 1);
    hilbert_transform_save_fir_state((uint32_t*)cap_buf[1]);
    fir_resample_x4((uint32_t*)fir_state, (uint32_t*)cic_buf, cic_len);
    cic_interpolate_x10(&cic, (uint32_t*)cic_buf, cic_len, (uint32_t*)dma_buf[1]);

    hilbert_transform((uint32_t*)cap_buf[0], (uint32_t*)fir_buf, sizeof fir_buf / sizeof(uint32_t), 1);
    hilbert_transform_save_fir_state((uint32_t*)cap_buf[1]);
    fir_resample_x4((uint32_t*)fir_state, (uint32_t*)cic_buf, cic_len);
    cic_interpolate_x10(&cic, (uint32_t*)cic_buf, cic_len, (uint32_t*)dma_buf[0]);
    
    hilbert_transform((uint32_t*)cap_buf[0], (uint32_t*)fir_buf, sizeof fir_buf / sizeof(uint32_t), 1);
    hilbert_transform_save_fir_state((uint32_t*)cap_buf[1]);
    fir_resample_x4((uint32_t*)fir_state, (uint32_t*)cic_buf, cic_len);
    cic_interpolate_x10(&cic, (uint32_t*)cic_buf, cic_len, (uint32_t*)dma_buf[1]);
#if 0
    for (int i = 0; i < 20; i++){
        pc.printf("%d, ", cic_buf[i*2]);
    }
    pc.printf("\n\r");
#endif
#if 1
    for (int i = 0; i < 30; i += 2) {
        pc.printf("%d:%d ", dma_buf[0][i], dma_buf[0][i+1]);
    }
    pc.printf("\n\r");
    int acc_i = 0;
    int acc_q = 0;
    for (int i = 0; i < DMA_DATALEN; i += 2){
        acc_i += dma_buf[0][i  ];
        acc_q += dma_buf[0][i+1];
    }
    pc.printf("dma acc %d %d\n\r", acc_i, acc_q);
#endif
}