this works!

commaai · Nov 14, 2024 · 71fc263 · 71fc263
1 parent a4ba125
commit 71fc263
Show file tree

Hide file tree

Showing 3 changed files with 45 additions and 17 deletions.
diff --git a/board/main.c b/board/main.c
@@ -171,10 +171,6 @@ static void tick_handler(void) {
       set_power_save_state(power_save_status);
     }
 
-    #ifdef STM32H7
-    print("mic dat: "); puth(DFSDM1_Filter0->FLTRDATAR >> 8); print("\n");
-    #endif
-
     // decimated to 1Hz
     if (loop_counter == 0U) {
       can_live = pending_can_live;

diff --git a/board/stm32h7/clock.h b/board/stm32h7/clock.h
@@ -109,6 +109,8 @@ void clock_init(void) {
   register_set(&(RCC->D2CCIP2R), RCC_D2CCIP2R_USBSEL_1 | RCC_D2CCIP2R_USBSEL_0, RCC_D2CCIP2R_USBSEL);
   // Configure clock source for FDCAN (PLL1Q at 80Mhz)
   register_set(&(RCC->D2CCIP1R), RCC_D2CCIP1R_FDCANSEL_0, RCC_D2CCIP1R_FDCANSEL);
+  // Configure clock source for DFSDM1
+  register_set_bits(&(RCC->D2CCIP1R), RCC_D2CCIP1R_DFSDM1SEL);
   // Configure clock source for ADC1,2,3 (per_ck(currently HSE))
   register_set(&(RCC->D3CCIPR), RCC_D3CCIPR_ADCSEL_1, RCC_D3CCIPR_ADCSEL);
   //Enable the Clock Security System

diff --git a/board/stm32h7/sound.h b/board/stm32h7/sound.h
@@ -1,7 +1,7 @@
 #define SOUND_RX_BUF_SIZE 2000U
 #define SOUND_TX_BUF_SIZE (SOUND_RX_BUF_SIZE/2U)
-#define MIC_RX_BUF_SIZE 1000U
-
+#define MIC_RX_BUF_SIZE 512U
+#define MIC_TX_BUF_SIZE (MIC_RX_BUF_SIZE * 2U)
 __attribute__((section(".sram4"))) static uint16_t sound_rx_buf[2][SOUND_RX_BUF_SIZE];
 __attribute__((section(".sram4"))) static uint32_t mic_rx_buf[2][MIC_RX_BUF_SIZE];
 
@@ -32,6 +32,26 @@ static void BDMA_Channel0_IRQ_Handler(void) {
   DMA1_Stream1->CR |= DMA_SxCR_EN;
 }
 
+// Recording processing
+static void DMA1_Stream0_IRQ_Handler(void) {
+  __attribute__((section(".sram4"))) static uint16_t tx_buf[MIC_TX_BUF_SIZE];
+
+  DMA1->LIFCR |= 0x7D; // clear flags
+
+  // process samples
+  uint8_t buf_idx = ((DMA1_Stream0->CR & DMA_SxCR_CT) >> DMA_SxCR_CT_Pos) == 1U ? 0U : 1U;
+  for (uint16_t i=0U; i < MIC_RX_BUF_SIZE; i++) {
+    tx_buf[2U*i] = ((mic_rx_buf[buf_idx][i] >> 16U) & 0xFFFF);
+    tx_buf[(2U*i)+1U] = tx_buf[2U*i];
+  }
+
+  BDMA->IFCR |= BDMA_IFCR_CGIF1;
+  BDMA_Channel1->CCR &= ~BDMA_CCR_EN;
+  register_set(&BDMA_Channel1->CM0AR, (uint32_t) tx_buf, 0xFFFFFFFFU);
+  BDMA_Channel1->CNDTR = MIC_TX_BUF_SIZE;
+  BDMA_Channel1->CCR |= BDMA_CCR_EN;
+}
+
 void sound_tick(void) {
   if (sound_idle_count > 0U) {
     sound_idle_count--;
@@ -43,6 +63,7 @@ void sound_tick(void) {
 
 void sound_init(void) {
   REGISTER_INTERRUPT(BDMA_Channel0_IRQn, BDMA_Channel0_IRQ_Handler, 64U, FAULT_INTERRUPT_RATE_SOUND_DMA)
+  REGISTER_INTERRUPT(DMA1_Stream0_IRQn, DMA1_Stream0_IRQ_Handler, 128U, FAULT_INTERRUPT_RATE_SOUND_DMA)
 
   // Init DAC
   register_set(&DAC1->MCR, 0U, 0xFFFFFFFFU);
@@ -91,23 +112,32 @@ void sound_init(void) {
   register_set(&SAI4_Block_A->SLOTR, (0b11 << SAI_xSLOTR_SLOTEN_Pos) | (1U << SAI_xSLOTR_NBSLOT_Pos) | (0b01 << SAI_xSLOTR_SLOTSZ_Pos), 0xFFFF0FDFU); // NBSLOT definition is vague
 
   // init DFSDM for PDM mic
-  DFSDM1_Channel0->CHCFGR1 = (6U << DFSDM_CHCFGR1_CKOUTDIV_Pos) | DFSDM_CHCFGR1_CHEN; // CH0 controls the clock
-  DFSDM1_Channel0->CHCFGR1 |= DFSDM_CHCFGR1_DFSDMEN;
+  DFSDM1_Channel0->CHCFGR1 = (76U << DFSDM_CHCFGR1_CKOUTDIV_Pos) | DFSDM_CHCFGR1_CHEN; // CH0 controls the clock
   DFSDM1_Channel3->CHCFGR1 |= (0b01 << DFSDM_CHCFGR1_SPICKSEL_Pos) | (0b00U << DFSDM_CHCFGR1_SITP_Pos) | DFSDM_CHCFGR1_CHEN; // SITP determines sample edge
-  DFSDM1_Filter0->FLTFCR = (64U << DFSDM_FLTFCR_FOSR_Pos) | (4 << DFSDM_FLTFCR_FORD_Pos);
-  DFSDM1_Filter0->FLTCR1 = (3U << DFSDM_FLTCR1_RCH_Pos) | DFSDM_FLTCR1_RCONT | DFSDM_FLTCR1_DFEN;
+  DFSDM1_Channel3->CHCFGR2 = (2U << DFSDM_CHCFGR2_DTRBS_Pos);
+  DFSDM1_Filter0->FLTFCR = (0U << DFSDM_FLTFCR_IOSR_Pos) | (64U << DFSDM_FLTFCR_FOSR_Pos) | (4U << DFSDM_FLTFCR_FORD_Pos);
+  DFSDM1_Filter0->FLTCR1 = DFSDM_FLTCR1_FAST | (3U << DFSDM_FLTCR1_RCH_Pos) | DFSDM_FLTCR1_RDMAEN | DFSDM_FLTCR1_RCONT | DFSDM_FLTCR1_DFEN;
+  DFSDM1_Channel0->CHCFGR1 |= DFSDM_CHCFGR1_DFSDMEN;
   DFSDM1_Filter0->FLTCR1 |= DFSDM_FLTCR1_RSWSTART;
 
-  // init DMA for SAI output
-  DMA1_Stream0->CR = DMA_SxCR_DBM | (8 << DMA_SxCR_CHSEL_Pos) | DMA_SxCR_PL_1 | DMA_SxCR_MSIZE_1 | (0b10UL << DMA_SxCR_PSIZE_Pos) | DMA_SxCR_CIRC;
-  DMA1_Stream0->PAR = (uint32_t) &DFSDM1_Filter0->FLTRDATAR;
-  DMA1_Stream0->M0AR = (uint32_t)&mic_rx_buf[0];
-  DMA1_Stream0->M1AR = (uint32_t)&mic_rx_buf[1];
+  // DMA (DFSDM1 -> memory)
+  DMA1_Stream0->PAR = (uint32_t)&DFSDM1_Filter0->FLTRDATAR;
+  DMA1_Stream0->M0AR = (uint32_t)mic_rx_buf[0];
+  DMA1_Stream0->M1AR = (uint32_t)mic_rx_buf[1];
+  DMA1_Stream0->NDTR = MIC_RX_BUF_SIZE;
+  DMA1_Stream0->CR = DMA_SxCR_DBM | (0b10UL << DMA_SxCR_MSIZE_Pos) | (0b10UL << DMA_SxCR_PSIZE_Pos) | DMA_SxCR_MINC | DMA_SxCR_CIRC | DMA_SxCR_TCIE;
+  register_set(&DMAMUX1_Channel0->CCR, 101U, DMAMUX_CxCR_DMAREQ_ID_Msk); // DFSDM1_DMA0
   DMA1_Stream0->CR |= DMA_SxCR_EN;
-  DMAMUX1_Channel0->CC |= 101U;
-  DFSDM1_Filter0->FLTCR2 |= DFSDM_FLTCR2_DMAEN;
+  DMA1->LIFCR |= 0x7D; // clear flags
+
+  // DMA (memory -> SAI4)
+  register_set(&BDMA_Channel1->CPAR, (uint32_t) &(SAI4_Block_A->DR), 0xFFFFFFFFU);
+  register_set(&BDMA_Channel1->CCR, (0b01 << BDMA_CCR_MSIZE_Pos) | (0b01 << BDMA_CCR_PSIZE_Pos) | BDMA_CCR_MINC | (0b1 << BDMA_CCR_DIR_Pos), 0xFFFEU);
+  register_set(&DMAMUX2_Channel1->CCR, 15U, DMAMUX_CxCR_DMAREQ_ID_Msk); // SAI4_A_DMA
 
   // enable all initted blocks
+  register_set_bits(&SAI4_Block_A->CR1, SAI_xCR1_SAIEN);
   register_set_bits(&SAI4_Block_B->CR1, SAI_xCR1_SAIEN);
   NVIC_EnableIRQ(BDMA_Channel0_IRQn);
+  NVIC_EnableIRQ(DMA1_Stream0_IRQn);
 }