Cuatro DFSDM1 mic demodulation (#2077)

* wip * this works! * pin todo * SAI1 isn't used * make it compile after rebase * fix gpio * use register functions and clean up misra * add power management for mic --------- Co-authored-by: Comma Device <[email protected]>
commaai · Jan 29, 2025 · c5c194f · c5c194f
1 parent 1290588
commit c5c194f
Show file tree

Hide file tree

Showing 4 changed files with 88 additions and 11 deletions.
diff --git a/board/boards/cuatro.h b/board/boards/cuatro.h
@@ -9,13 +9,13 @@
 static void cuatro_set_led(uint8_t color, bool enabled) {
   switch (color) {
     case LED_RED:
-      set_gpio_output(GPIOD, 15, !enabled);
+      set_gpio_output(GPIOC, 6, !enabled);
       break;
     case LED_GREEN:
-      set_gpio_output(GPIOB, 2, !enabled);
+      set_gpio_output(GPIOC, 7, !enabled);
       break;
     case LED_BLUE:
-      set_gpio_output(GPIOE, 2, !enabled);
+      set_gpio_output(GPIOC, 9, !enabled);
       break;
     default:
       break;
@@ -79,9 +79,9 @@ static void cuatro_init(void) {
   red_chiplet_init();
 
   // init LEDs as open drain
-  set_gpio_output_type(GPIOE, 2, OUTPUT_TYPE_OPEN_DRAIN);
-  set_gpio_output_type(GPIOB, 2, OUTPUT_TYPE_OPEN_DRAIN);
-  set_gpio_output_type(GPIOD, 15, OUTPUT_TYPE_OPEN_DRAIN);
+  set_gpio_output_type(GPIOC, 6, OUTPUT_TYPE_OPEN_DRAIN);
+  set_gpio_output_type(GPIOC, 7, OUTPUT_TYPE_OPEN_DRAIN);
+  set_gpio_output_type(GPIOC, 9, OUTPUT_TYPE_OPEN_DRAIN);
 
   // more open drain
   set_gpio_output_type(GPIOD, 3, OUTPUT_TYPE_OPEN_DRAIN); // FAN_EN
@@ -135,8 +135,8 @@ static void cuatro_init(void) {
   set_gpio_alternate(GPIOC, 0, GPIO_AF8_SAI4);    // SAI4_FS_B
   set_gpio_alternate(GPIOD, 11, GPIO_AF10_SAI4);  // SAI4_SD_A
   set_gpio_alternate(GPIOE, 3, GPIO_AF8_SAI4);    // SAI4_SD_B
-  set_gpio_alternate(GPIOE, 4, GPIO_AF2_SAI1);    // SAI1_D2
-  set_gpio_alternate(GPIOE, 5, GPIO_AF2_SAI1);    // SAI1_CK2
+  set_gpio_alternate(GPIOE, 4, GPIO_AF3_DFSDM1);  // DFSDM1_DATIN3
+  set_gpio_alternate(GPIOE, 9, GPIO_AF3_DFSDM1);  // DFSDM1_CKOUT
   set_gpio_alternate(GPIOE, 6, GPIO_AF10_SAI4);   // SAI4_MCLK_B
   sound_init();
 }

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/peripherals.h b/board/stm32h7/peripherals.h
@@ -124,6 +124,7 @@ void peripherals_init(void) {
   RCC->APB1LENR |= RCC_APB1LENR_DAC12EN; // DAC
 
   // Audio
+  RCC->APB2ENR |= RCC_APB2ENR_DFSDM1EN; // D/S demodulator for mic
   RCC->APB4ENR |= RCC_APB4ENR_SAI4EN;  // SAI4
 
   // Timers

diff --git a/board/stm32h7/sound.h b/board/stm32h7/sound.h
@@ -1,10 +1,14 @@
-#define SOUND_RX_BUF_SIZE 2000U
+#define SOUND_RX_BUF_SIZE 1000U
 #define SOUND_TX_BUF_SIZE (SOUND_RX_BUF_SIZE/2U)
-
+#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 uint16_t sound_tx_buf[2][SOUND_TX_BUF_SIZE];
+__attribute__((section(".sram4"))) static uint32_t mic_rx_buf[2][MIC_RX_BUF_SIZE];
 
+#define SOUND_IDLE_TIMEOUT 4U
 static uint8_t sound_idle_count;
+static uint8_t mic_idle_count;
 
 void sound_tick(void) {
   if (sound_idle_count > 0U) {
@@ -14,6 +18,33 @@ void sound_tick(void) {
       register_clear_bits(&DMA1_Stream1->CR, DMA_SxCR_EN);
     }
   }
+
+  if (mic_idle_count > 0U) {
+    mic_idle_count--;
+    if (mic_idle_count == 0U) {
+      register_clear_bits(&DFSDM1_Channel0->CHCFGR1, DFSDM_CHCFGR1_DFSDMEN);
+    }
+  }
+}
+
+// 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) & 0xFFFFU);
+    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;
 }
 
 // Playback processing
@@ -45,13 +76,22 @@ static void BDMA_Channel0_IRQ_Handler(void) {
       register_set(&DMA1_Stream1->CR, (1UL - tx_buf_idx) << DMA_SxCR_CT_Pos, DMA_SxCR_CT_Msk);
       register_set_bits(&DMA1_Stream1->CR, DMA_SxCR_EN);
     }
-    sound_idle_count = 4U;
+    sound_idle_count = SOUND_IDLE_TIMEOUT;
+  }
+
+  // manage mic state
+  if (mic_idle_count == 0U) {
+    register_set_bits(&DFSDM1_Channel0->CHCFGR1, DFSDM_CHCFGR1_DFSDMEN);
+    DFSDM1_Filter0->FLTCR1 |= DFSDM_FLTCR1_RSWSTART;
   }
+  mic_idle_count = SOUND_IDLE_TIMEOUT;
+
   sound_tick();
 }
 
 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);
@@ -78,6 +118,10 @@ void sound_init(void) {
   TIM5->CNT = 0U; TIM5->SR = 0U;
   TIM5->CR1 |= TIM_CR1_CEN;
 
+  // sync both SAIs
+  register_set(&SAI4->GCR, (0b10UL << SAI_GCR_SYNCOUT_Pos), SAI_GCR_SYNCIN_Msk | SAI_GCR_SYNCOUT_Msk);
+  register_set(&SAI1->GCR, (3U << SAI_GCR_SYNCIN_Pos), SAI_GCR_SYNCIN_Msk | SAI_GCR_SYNCOUT_Msk);
+
   // stereo audio in
   register_set(&SAI4_Block_B->CR1, SAI_xCR1_DMAEN | (0b00UL << SAI_xCR1_SYNCEN_Pos) | (0b100U << SAI_xCR1_DS_Pos) | (0b11U << SAI_xCR1_MODE_Pos), 0x0FFB3FEFU);
   register_set(&SAI4_Block_B->CR2, (0b001U << SAI_xCR2_FTH_Pos), 0xFFFBU);
@@ -93,7 +137,37 @@ void sound_init(void) {
   register_set(&DMAMUX2_Channel0->CCR, 16U, DMAMUX_CxCR_DMAREQ_ID_Msk); // SAI4_B_DMA
   register_set_bits(&BDMA_Channel0->CCR, BDMA_CCR_EN);
 
+  // mic output
+  register_set(&SAI4_Block_A->CR1, SAI_xCR1_DMAEN | (0b01UL << SAI_xCR1_SYNCEN_Pos) | (0b100UL << SAI_xCR1_DS_Pos) | (0b10UL << SAI_xCR1_MODE_Pos), 0x0FFB3FEFU);
+  register_set(&SAI4_Block_A->CR2, 0U, 0xFFFBU);
+  register_set(&SAI4_Block_A->FRCR, (31U << SAI_xFRCR_FRL_Pos), 0x7FFFFU);
+  register_set(&SAI4_Block_A->SLOTR, (0b11UL << SAI_xSLOTR_SLOTEN_Pos) | (1UL << SAI_xSLOTR_NBSLOT_Pos) | (0b01U << SAI_xSLOTR_SLOTSZ_Pos), 0xFFFF0FDFU); // NBSLOT definition is vague
+
+  // init DFSDM for PDM mic
+  register_set(&DFSDM1_Channel0->CHCFGR1, (76UL << DFSDM_CHCFGR1_CKOUTDIV_Pos) | DFSDM_CHCFGR1_CHEN, 0xC0FFF1EFU); // CH0 controls the clock
+  register_set(&DFSDM1_Channel3->CHCFGR1, (0b01UL << DFSDM_CHCFGR1_SPICKSEL_Pos) | (0b00U << DFSDM_CHCFGR1_SITP_Pos) | DFSDM_CHCFGR1_CHEN, 0x0000F1EFU); // SITP determines sample edge
+  register_set(&DFSDM1_Channel3->CHCFGR2, (2U << DFSDM_CHCFGR2_DTRBS_Pos), 0xFFFFFFF7U);
+  register_set(&DFSDM1_Filter0->FLTFCR, (0U << DFSDM_FLTFCR_IOSR_Pos) | (64UL << DFSDM_FLTFCR_FOSR_Pos) | (4UL << DFSDM_FLTFCR_FORD_Pos), 0xE3FF00FFU);
+  register_set(&DFSDM1_Filter0->FLTCR1, DFSDM_FLTCR1_FAST | (3UL << DFSDM_FLTCR1_RCH_Pos) | DFSDM_FLTCR1_RDMAEN | DFSDM_FLTCR1_RCONT | DFSDM_FLTCR1_DFEN, 0x672E7F3BU);
+
+  // DMA (DFSDM1 -> memory)
+  register_set(&DMA1_Stream0->PAR, (uint32_t) &DFSDM1_Filter0->FLTRDATAR, 0xFFFFFFFFU);
+  register_set(&DMA1_Stream0->M0AR, (uint32_t)mic_rx_buf[0], 0xFFFFFFFFU);
+  register_set(&DMA1_Stream0->M1AR, (uint32_t)mic_rx_buf[1], 0xFFFFFFFFU);
+  DMA1_Stream0->NDTR = MIC_RX_BUF_SIZE;
+  register_set(&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, 0x01FFFFFFU);
+  register_set(&DMAMUX1_Channel0->CCR, 101U, DMAMUX_CxCR_DMAREQ_ID_Msk); // DFSDM1_DMA0
+  register_set_bits(&DMA1_Stream0->CR, DMA_SxCR_EN);
+  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, (0b01UL << BDMA_CCR_MSIZE_Pos) | (0b01UL << BDMA_CCR_PSIZE_Pos) | BDMA_CCR_MINC | (0b1U << 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);
 }