Diff: main.cpp

Revision:

0:55201637d936

Child:

1:457ef59cce95

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Mar 21 08:31:20 2015 +0000
@@ -0,0 +1,712 @@
+#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;
+
+
+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
+}
+
+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);
+    i = __SMUAD(0x0001000f, i) >> 4;
+    q = __SMUAD(0x0001000f, q) >> 4;
+    decay_ave = __PKHBT(i, 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_USB;
+
+const char *mode_names[] = {
+    "NFM", "AM", "USB", "LSB", "IQ"
+};
+
+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);
+#if 0
+    pc.printf("%08x\r\n", cos_sin(0x8000));
+    pc.printf("%08x\r\n", cos_sin(0xC000));
+    pc.printf("%08x\r\n", cos_sin(0xFF7F));
+    pc.printf("%08x\r\n", cos_sin(0x0));
+    pc.printf("%08x\r\n", cos_sin(0x0080));
+    pc.printf("%08x\r\n", cos_sin(0x4000));
+    pc.printf("%08x\r\n", cos_sin(0x7fff));
+#endif
+    // 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(3, 3, 16); // adjust offset
+    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));
+        //pc.printf("%d %d\r\n", dma_buf[0][100], dma_buf[0][101]);
+        //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(0);
+    decay_average(capbuf_ave);
+    modulate_buffer_half(1);
+}