cuatro sound (#1861)

* fake siren works * this receives something * microphone in SAI1 POC * this receives audio again * double buffer DMA? * RX DMA works * wip * needs cleanup, but this plays * cleanup of playback * print mic * deal with stereo in * the DMA from SAI1 -> SAI4 doesn't work yet * this puts mic data in a buf * this gets sound to the 845 * wip: still garbage from the mic * inefficient, but it does record the mic * sine isn't used * comment out mic for now * fix misra * remove mic * more cleanup * add amp enable/disable * no more debug * newline --------- Co-authored-by: Comma Device <[email protected]>
commaai · Nov 13, 2024 · dec9223 · dec9223
1 parent ae4f753
commit dec9223
Show file tree

Hide file tree

Showing 16 changed files with 143 additions and 9 deletions.
diff --git a/board/boards/black.h b/board/boards/black.h
@@ -182,5 +182,6 @@ board board_black = {
   .set_fan_enabled = unused_set_fan_enabled,
   .set_ir_power = unused_set_ir_power,
   .set_siren = unused_set_siren,
-  .read_som_gpio = unused_read_som_gpio
+  .read_som_gpio = unused_read_som_gpio,
+  .set_amp_enabled = unused_set_amp_enabled
 };
diff --git a/board/boards/board_declarations.h b/board/boards/board_declarations.h
@@ -24,6 +24,7 @@ typedef void (*board_set_fan_enabled)(bool enabled);
 typedef void (*board_set_siren)(bool enabled);
 typedef void (*board_set_bootkick)(BootState state);
 typedef bool (*board_read_som_gpio)(void);
+typedef void (*board_set_amp_enabled)(bool enabled);
 
 struct board {
   harness_configuration *harness_config;
@@ -49,6 +50,7 @@ struct board {
   board_set_siren set_siren;
   board_set_bootkick set_bootkick;
   board_read_som_gpio read_som_gpio;
+  board_set_amp_enabled set_amp_enabled;
 };
 
 // ******************* Definitions ********************

diff --git a/board/boards/cuatro.h b/board/boards/cuatro.h
@@ -75,6 +75,10 @@ static void cuatro_set_siren(bool enabled){
   beeper_enable(enabled);
 }
 
+static void cuatro_set_amp_enabled(bool enabled){
+  set_gpio_output(GPIOA, 5, enabled);
+}
+
 static void cuatro_init(void) {
   red_chiplet_init();
 
@@ -128,6 +132,17 @@ static void cuatro_init(void) {
   // Beeper
   set_gpio_alternate(GPIOD, 14, GPIO_AF2_TIM4);
   beeper_init();
+
+  // Sound codec
+  cuatro_set_amp_enabled(false);
+  set_gpio_alternate(GPIOA, 2, GPIO_AF8_SAI4);    // SAI4_SCK_B
+  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, 6, GPIO_AF10_SAI4);   // SAI4_MCLK_B
+  sound_init();
 }
 
 board board_cuatro = {
@@ -153,5 +168,6 @@ board board_cuatro = {
   .set_ir_power = tres_set_ir_power,
   .set_siren = cuatro_set_siren,
   .set_bootkick = cuatro_set_bootkick,
-  .read_som_gpio = tres_read_som_gpio
+  .read_som_gpio = tres_read_som_gpio,
+  .set_amp_enabled = cuatro_set_amp_enabled
 };
diff --git a/board/boards/dos.h b/board/boards/dos.h
@@ -211,5 +211,6 @@ board board_dos = {
   .set_ir_power = dos_set_ir_power,
   .set_siren = dos_set_siren,
   .set_bootkick = dos_set_bootkick,
-  .read_som_gpio = dos_read_som_gpio
+  .read_som_gpio = dos_read_som_gpio,
+  .set_amp_enabled = unused_set_amp_enabled
 };
diff --git a/board/boards/grey.h b/board/boards/grey.h
@@ -31,5 +31,6 @@ board board_grey = {
   .set_fan_enabled = unused_set_fan_enabled,
   .set_ir_power = unused_set_ir_power,
   .set_siren = unused_set_siren,
-  .read_som_gpio = unused_read_som_gpio
+  .read_som_gpio = unused_read_som_gpio,
+  .set_amp_enabled = unused_set_amp_enabled
 };
diff --git a/board/boards/red.h b/board/boards/red.h
@@ -194,5 +194,6 @@ board board_red = {
   .set_fan_enabled = unused_set_fan_enabled,
   .set_ir_power = unused_set_ir_power,
   .set_siren = unused_set_siren,
-  .read_som_gpio = unused_read_som_gpio
+  .read_som_gpio = unused_read_som_gpio,
+  .set_amp_enabled = unused_set_amp_enabled
 };
diff --git a/board/boards/tres.h b/board/boards/tres.h
@@ -97,5 +97,6 @@ board board_tres = {
   .set_ir_power = tres_set_ir_power,
   .set_siren = fake_siren_set,
   .set_bootkick = tres_set_bootkick,
-  .read_som_gpio = tres_read_som_gpio
+  .read_som_gpio = tres_read_som_gpio,
+  .set_amp_enabled = unused_set_amp_enabled
 };
diff --git a/board/boards/uno.h b/board/boards/uno.h
@@ -218,5 +218,6 @@ board board_uno = {
   .set_ir_power = uno_set_ir_power,
   .set_siren = unused_set_siren,
   .set_bootkick = uno_set_bootkick,
-  .read_som_gpio = unused_read_som_gpio
+  .read_som_gpio = unused_read_som_gpio,
+  .set_amp_enabled = unused_set_amp_enabled
 };
diff --git a/board/boards/unused_funcs.h b/board/boards/unused_funcs.h
@@ -26,3 +26,7 @@ void unused_set_bootkick(BootState state) {
 bool unused_read_som_gpio(void) {
   return false;
 }
+
+void unused_set_amp_enabled(bool enabled) {
+  UNUSED(enabled);
+}
diff --git a/board/boards/white.h b/board/boards/white.h
@@ -211,5 +211,6 @@ board board_white = {
   .set_fan_enabled = unused_set_fan_enabled,
   .set_ir_power = unused_set_ir_power,
   .set_siren = unused_set_siren,
-  .read_som_gpio = unused_read_som_gpio
+  .read_som_gpio = unused_read_som_gpio,
+  .set_amp_enabled = unused_set_amp_enabled
 };
diff --git a/board/faults_declarations.h b/board/faults_declarations.h
@@ -32,6 +32,7 @@
 #define FAULT_INTERRUPT_RATE_UART_7         (1UL << 24)
 #define FAULT_SIREN_MALFUNCTION             (1UL << 25)
 #define FAULT_HEARTBEAT_LOOP_WATCHDOG       (1UL << 26)
+#define FAULT_INTERRUPT_RATE_SOUND_DMA      (1UL << 27)
 
 // Permanent faults
 #define PERMANENT_FAULTS 0U

diff --git a/board/main.c b/board/main.c
@@ -158,6 +158,7 @@ static void tick_handler(void) {
     usb_tick();
     harness_tick();
     simple_watchdog_kick();
+    sound_tick();
 
     // re-init everything that uses harness status
     if (harness.status != prev_harness_status) {

diff --git a/board/stm32f4/board.h b/board/stm32f4/board.h
@@ -16,6 +16,9 @@
 #include "boards/uno.h"
 #include "boards/dos.h"
 
+// Unused functions on F4
+void sound_tick(void) {}
+
 void detect_board_type(void) {
   // SPI lines floating: white (TODO: is this reliable? Not really, we have to enable ESP/GPS to be able to detect this on the UART)
   set_gpio_output(GPIOC, 14, 1);

diff --git a/board/stm32h7/board.h b/board/stm32h7/board.h
@@ -12,6 +12,7 @@
 #include "stm32h7/lldac.h"
 #include "drivers/beeper.h"
 #include "drivers/fake_siren.h"
+#include "stm32h7/sound.h"
 #include "drivers/clock_source.h"
 #include "boards/red.h"
 #include "boards/red_chiplet.h"

diff --git a/board/stm32h7/peripherals.h b/board/stm32h7/peripherals.h
@@ -101,13 +101,14 @@ void peripherals_init(void) {
   RCC->AHB4ENR |= RCC_AHB4ENR_GPIOFEN;
   RCC->AHB4ENR |= RCC_AHB4ENR_GPIOGEN;
 
-  // Enable CPU access to SRAM1 and SRAM2 (in domain D2) for DMA
+  // Enable CPU access to SRAMs for DMA
   RCC->AHB2ENR |= RCC_AHB2ENR_SRAM1EN | RCC_AHB2ENR_SRAM2EN;
 
   // Supplemental
   RCC->AHB1ENR |= RCC_AHB1ENR_DMA1EN;  // DAC DMA
   RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN;  // SPI DMA
   RCC->APB4ENR |= RCC_APB4ENR_SYSCFGEN;
+  RCC->AHB4ENR |= RCC_AHB4ENR_BDMAEN; // Audio DMA
 
   // Connectivity
   RCC->APB2ENR |= RCC_APB2ENR_SPI4EN;  // SPI
@@ -122,6 +123,9 @@ void peripherals_init(void) {
   RCC->AHB1ENR |= RCC_AHB1ENR_ADC12EN; // Enable ADC12 clocks
   RCC->APB1LENR |= RCC_APB1LENR_DAC12EN; // DAC
 
+  // Audio
+  RCC->APB4ENR |= RCC_APB4ENR_SAI4EN;  // SAI4
+
   // Timers
   RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;  // clock source timer
   RCC->APB1LENR |= RCC_APB1LENR_TIM2EN;  // main counter

diff --git a/board/stm32h7/sound.h b/board/stm32h7/sound.h
@@ -0,0 +1,95 @@
+
+#define SOUND_RX_BUF_SIZE 2000U
+#define SOUND_TX_BUF_SIZE (SOUND_RX_BUF_SIZE/2U)
+
+__attribute__((section(".sram4"))) static uint16_t sound_rx_buf[2][SOUND_RX_BUF_SIZE];
+
+typedef enum {
+  OFF = 0,
+  IDLE = 1,
+  PLAYING = 2,
+} SoundStatus;
+static SoundStatus sound_status = OFF;
+
+// Playback processing
+static void BDMA_Channel0_IRQ_Handler(void) {
+  __attribute__((section(".sram4"))) static uint16_t tx_buf[SOUND_TX_BUF_SIZE];
+
+  BDMA->IFCR |= BDMA_IFCR_CGIF0; // clear flag
+
+  // process samples (shift to 12b and bias to be unsigned)
+  // since we are playing mono and receiving stereo, we take every other sample
+  uint8_t buf_idx = (((BDMA_Channel0->CCR & BDMA_CCR_CT) >> BDMA_CCR_CT_Pos) == 1U) ? 0U : 1U;
+  for (uint16_t i=0U; i < SOUND_RX_BUF_SIZE; i += 2U) {
+    tx_buf[i/2U] = ((sound_rx_buf[buf_idx][i] + (1UL << 14)) >> 3);
+  }
+
+  if (sound_status == OFF) {
+    current_board->set_amp_enabled(true);
+  }
+  sound_status = PLAYING;
+
+  DMA1->LIFCR |= 0xF40;
+  DMA1_Stream1->CR &= ~DMA_SxCR_EN;
+  register_set(&DMA1_Stream1->M0AR, (uint32_t) tx_buf, 0xFFFFFFFFU);
+  DMA1_Stream1->NDTR = SOUND_TX_BUF_SIZE;
+  DMA1_Stream1->CR |= DMA_SxCR_EN;
+}
+
+void sound_tick(void) {
+  switch (sound_status) {
+    case IDLE:
+      current_board->set_amp_enabled(false);
+      sound_status = OFF;
+      break;
+    case PLAYING:
+      sound_status = IDLE;
+      break;
+    default:
+      break;
+  }
+}
+
+void sound_init(void) {
+  REGISTER_INTERRUPT(BDMA_Channel0_IRQn, BDMA_Channel0_IRQ_Handler, 64U, FAULT_INTERRUPT_RATE_SOUND_DMA)
+
+  // Init DAC
+  register_set(&DAC1->MCR, 0U, 0xFFFFFFFFU);
+  register_set(&DAC1->CR, DAC_CR_TEN1 | (6U << DAC_CR_TSEL1_Pos) | DAC_CR_DMAEN1, 0xFFFFFFFFU);
+  register_set_bits(&DAC1->CR, DAC_CR_EN1);
+
+  // Setup DMAMUX (DAC_CH1_DMA as input)
+  register_set(&DMAMUX1_Channel1->CCR, 67U, DMAMUX_CxCR_DMAREQ_ID_Msk);
+
+  // Setup DMA
+  register_set(&DMA1_Stream1->PAR, (uint32_t) &(DAC1->DHR12R1), 0xFFFFFFFFU);
+  register_set(&DMA1_Stream1->FCR, 0U, 0x00000083U);
+  DMA1_Stream1->CR = (0b11UL << DMA_SxCR_PL_Pos) | (0b01UL << DMA_SxCR_MSIZE_Pos) | (0b01UL << DMA_SxCR_PSIZE_Pos) | DMA_SxCR_MINC | (1U << DMA_SxCR_DIR_Pos);
+
+  // Init trigger timer (48kHz)
+  register_set(&TIM7->PSC, 0U, 0xFFFFU);
+  register_set(&TIM7->ARR, 2494U, 0xFFFFU);
+  register_set(&TIM7->CR2, (0b10U << TIM_CR2_MMS_Pos), TIM_CR2_MMS_Msk);
+  register_set(&TIM7->CR1, TIM_CR1_ARPE | TIM_CR1_URS, 0x088EU);
+  TIM7->SR = 0U;
+  TIM7->CR1 |= TIM_CR1_CEN;
+
+  // 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);
+  register_set(&SAI4_Block_B->FRCR, (31U << SAI_xFRCR_FRL_Pos), 0x7FFFFU);
+  register_set(&SAI4_Block_B->SLOTR, (0b11UL << SAI_xSLOTR_SLOTEN_Pos) | (1UL << SAI_xSLOTR_NBSLOT_Pos) | (0b01UL << SAI_xSLOTR_SLOTSZ_Pos), 0xFFFF0FDFU); // NBSLOT definition is vague
+
+  // init sound DMA (SAI4_B -> memory, double buffers)
+  register_set(&BDMA_Channel0->CPAR, (uint32_t) &(SAI4_Block_B->DR), 0xFFFFFFFFU);
+  register_set(&BDMA_Channel0->CM0AR, (uint32_t) sound_rx_buf[0], 0xFFFFFFFFU);
+  register_set(&BDMA_Channel0->CM1AR, (uint32_t) sound_rx_buf[1], 0xFFFFFFFFU);
+  BDMA_Channel0->CNDTR = SOUND_RX_BUF_SIZE;
+  register_set(&BDMA_Channel0->CCR, BDMA_CCR_DBM | (0b01UL << BDMA_CCR_MSIZE_Pos) | (0b01UL << BDMA_CCR_PSIZE_Pos) | BDMA_CCR_MINC | BDMA_CCR_CIRC | BDMA_CCR_TCIE, 0xFFFFU);
+  register_set(&DMAMUX2_Channel0->CCR, 16U, DMAMUX_CxCR_DMAREQ_ID_Msk); // SAI4_B_DMA
+  register_set_bits(&BDMA_Channel0->CCR, BDMA_CCR_EN);
+
+  // enable all initted blocks
+  register_set_bits(&SAI4_Block_B->CR1, SAI_xCR1_SAIEN);
+  NVIC_EnableIRQ(BDMA_Channel0_IRQn);
+}