Fake siren (commaai#1192)

* DAC POC * change freq * some cleanup * wip: cleaning up and trying to use DMA * no clue why this doesn't work * this works * wip multi-master i2c for fake siren * cleanup and some more i2c stuff * seems more stable * retry disabling and cleanup force siren * fix misra violations * not needed * messed up rebase Co-authored-by: Comma Device <device@comma.ai>
mlocoteta · Apr 29, 2023 · 708b42c · 708b42c
1 parent 66c7b99
commit 708b42c
Show file tree

Hide file tree

Showing 6 changed files with 282 additions and 1 deletion.
diff --git a/board/boards/tres.h b/board/boards/tres.h
@@ -53,6 +53,12 @@ void tres_init(void) {
   set_gpio_alternate(GPIOC, 9, GPIO_AF2_TIM3);
   pwm_init(TIM3, 4);
   tres_set_ir_power(0U);
+
+  // Fake siren
+  set_gpio_alternate(GPIOC, 10, GPIO_AF4_I2C5);
+  set_gpio_alternate(GPIOC, 11, GPIO_AF4_I2C5);
+  register_set_bits(&(GPIOC->OTYPER), GPIO_OTYPER_OT10 | GPIO_OTYPER_OT11); // open drain
+  fake_siren_init();
 }
 
 const board board_tres = {
@@ -79,5 +85,5 @@ const board board_tres = {
   .set_ir_power = tres_set_ir_power,
   .set_phone_power = unused_set_phone_power,
   .set_clock_source_mode = unused_set_clock_source_mode,
-  .set_siren = unused_set_siren
+  .set_siren = fake_siren_set
 };
diff --git a/board/drivers/fake_siren.h b/board/drivers/fake_siren.h
@@ -0,0 +1,74 @@
+#include "stm32h7/lli2c.h"
+
+#define CODEC_I2C_ADDR 0x10
+// 1Vpp sine wave with 1V offset
+const uint8_t fake_siren_lut[360] = { 134U, 135U, 137U, 138U, 139U, 140U, 141U, 143U, 144U, 145U, 146U, 148U, 149U, 150U, 151U, 152U, 154U, 155U, 156U, 157U, 158U, 159U, 160U, 162U, 163U, 164U, 165U, 166U, 167U, 168U, 169U, 170U, 171U, 172U, 174U, 175U, 176U, 177U, 177U, 178U, 179U, 180U, 181U, 182U, 183U, 184U, 185U, 186U, 186U, 187U, 188U, 189U, 190U, 190U, 191U, 192U, 193U, 193U, 194U, 195U, 195U, 196U, 196U, 197U, 197U, 198U, 199U, 199U, 199U, 200U, 200U, 201U, 201U, 202U, 202U, 202U, 203U, 203U, 203U, 203U, 204U, 204U, 204U, 204U, 204U, 204U, 204U, 205U, 205U, 205U, 205U, 205U, 205U, 205U, 204U, 204U, 204U, 204U, 204U, 204U, 204U, 203U, 203U, 203U, 203U, 202U, 202U, 202U, 201U, 201U, 200U, 200U, 199U, 199U, 199U, 198U, 197U, 197U, 196U, 196U, 195U, 195U, 194U, 193U, 193U, 192U, 191U, 190U, 190U, 189U, 188U, 187U, 186U, 186U, 185U, 184U, 183U, 182U, 181U, 180U, 179U, 178U, 177U, 177U, 176U, 175U, 174U, 172U, 171U, 170U, 169U, 168U, 167U, 166U, 165U, 164U, 163U, 162U, 160U, 159U, 158U, 157U, 156U, 155U, 154U, 152U, 151U, 150U, 149U, 148U, 146U, 145U, 144U, 143U, 141U, 140U, 139U, 138U, 137U, 135U, 134U, 133U, 132U, 130U, 129U, 128U, 127U, 125U, 124U, 123U, 122U, 121U, 119U, 118U, 117U, 116U, 115U, 113U, 112U, 111U, 110U, 109U, 108U, 106U, 105U, 104U, 103U, 102U, 101U, 100U, 99U, 98U, 97U, 96U, 95U, 94U, 93U, 92U, 91U, 90U, 89U, 88U, 87U, 86U, 85U, 84U, 83U, 82U, 82U, 81U, 80U, 79U, 78U, 78U, 77U, 76U, 76U, 75U, 74U, 74U, 73U, 72U, 72U, 71U, 71U, 70U, 70U, 69U, 69U, 68U, 68U, 67U, 67U, 67U, 66U, 66U, 66U, 65U, 65U, 65U, 65U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 63U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 64U, 65U, 65U, 65U, 65U, 66U, 66U, 66U, 67U, 67U, 67U, 68U, 68U, 69U, 69U, 70U, 70U, 71U, 71U, 72U, 72U, 73U, 74U, 74U, 75U, 76U, 76U, 77U, 78U, 78U, 79U, 80U, 81U, 82U, 82U, 83U, 84U, 85U, 86U, 87U, 88U, 89U, 90U, 91U, 92U, 93U, 94U, 95U, 96U, 97U, 98U, 99U, 100U, 101U, 102U, 103U, 104U, 105U, 106U, 108U, 109U, 110U, 111U, 112U, 113U, 115U, 116U, 117U, 118U, 119U, 121U, 122U, 123U, 124U, 125U, 127U, 128U, 129U, 130U, 132U, 133U };
+
+bool fake_siren_enabled = false;
+
+void fake_siren_codec_enable(bool enabled) {
+  if(enabled) {
+    bool success = false;
+    success &= i2c_set_reg_bits(I2C5, CODEC_I2C_ADDR, 0x2B, (1U << 1)); // Left speaker mix from INA1
+    success &= i2c_set_reg_bits(I2C5, CODEC_I2C_ADDR, 0x2C, (1U << 1)); // Right speaker mix from INA1
+    success &= i2c_set_reg_mask(I2C5, CODEC_I2C_ADDR, 0x3D, 0x1F, 0b11111); // Left speaker volume
+    success &= i2c_set_reg_mask(I2C5, CODEC_I2C_ADDR, 0x3E, 0x1F, 0b11111); // Right speaker volume
+    success &= i2c_set_reg_mask(I2C5, CODEC_I2C_ADDR, 0x37, 0b101, 0b111); // INA gain
+    success &= i2c_set_reg_bits(I2C5, CODEC_I2C_ADDR, 0x4C, (1U << 7)); // Enable INA
+    success &= i2c_set_reg_bits(I2C5, CODEC_I2C_ADDR, 0x51, (1U << 7)); // Disable global shutdown
+    if (!success) {
+      print("Failed to setup the codec for fake siren\n");
+    }
+  } else {
+    // Disable INA input. Make sure to retry a few times if the I2C bus is busy.
+    for(uint8_t i=0U; i<10U; i++) {
+      if (i2c_clear_reg_bits(I2C5, CODEC_I2C_ADDR, 0x4C, (1U << 7))) {
+        break;
+      }
+    }
+  }
+}
+
+
+void fake_siren_set(bool enabled) {
+  if (enabled != fake_siren_enabled) {
+    fake_siren_codec_enable(enabled);
+  }
+
+  if (enabled) {
+    register_set_bits(&DMA1_Stream1->CR, DMA_SxCR_EN);
+  } else {
+    register_clear_bits(&DMA1_Stream1->CR, DMA_SxCR_EN);
+  }
+  fake_siren_enabled = enabled;
+}
+
+void fake_siren_init(void) {
+  // 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->M0AR, (uint32_t) fake_siren_lut, 0xFFFFFFFFU);
+  register_set(&DMA1_Stream1->PAR, (uint32_t) &(DAC1->DHR8R1), 0xFFFFFFFFU);
+  DMA1_Stream1->NDTR = sizeof(fake_siren_lut);
+  register_set(&DMA1_Stream1->FCR, 0U, 0xFFFFFFFFU);
+  DMA1_Stream1->CR = (0b11 << DMA_SxCR_PL_Pos);
+  DMA1_Stream1->CR |= DMA_SxCR_MINC | DMA_SxCR_CIRC | (1 << DMA_SxCR_DIR_Pos);
+
+  // Init trigger timer (around 2.5kHz)
+  register_set(&TIM7->PSC, 0U, 0xFFFFU);
+  register_set(&TIM7->ARR, 133U, 0xFFFFU);
+  register_set(&TIM7->CR2, (0b10 << 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;
+
+  // Enable the I2C to the codec
+  i2c_init(I2C5);
+  fake_siren_codec_enable(false);
+}
diff --git a/board/stm32h7/board.h b/board/stm32h7/board.h
@@ -9,6 +9,8 @@
 #include "drivers/fan.h"
 #include "stm32h7/llfan.h"
 #include "stm32h7/llrtc.h"
+#include "stm32h7/lldac.h"
+#include "drivers/fake_siren.h"
 #include "drivers/rtc.h"
 #include "boards/red.h"
 #include "boards/red_chiplet.h"

diff --git a/board/stm32h7/lldac.h b/board/stm32h7/lldac.h
@@ -0,0 +1,42 @@
+void dac_init(DAC_TypeDef *dac, uint8_t channel, bool dma) {
+  register_set(&dac->CR, 0U, 0xFFFFU);
+  register_set(&dac->MCR, 0U, 0xFFFFU);
+
+  switch(channel) {
+    case 1:
+      if (dma) {
+        register_set_bits(&dac->CR, DAC_CR_DMAEN1);
+        // register_set(&DAC->CR, (6U << DAC_CR_TSEL1_Pos), DAC_CR_TSEL1);
+        register_set_bits(&dac->CR, DAC_CR_TEN1);
+      } else {
+        register_clear_bits(&dac->CR, DAC_CR_DMAEN1);
+      }
+      register_set_bits(&dac->CR, DAC_CR_EN1);
+      break;
+    case 2:    
+      if (dma) {
+        register_set_bits(&dac->CR, DAC_CR_DMAEN2);
+      } else {
+        register_clear_bits(&dac->CR, DAC_CR_DMAEN2);
+      }
+      register_set_bits(&dac->CR, DAC_CR_EN2);
+      break;
+    default:
+      break;
+  }
+}
+
+// Set channel 1 value, in mV
+void dac_set(DAC_TypeDef *dac, uint8_t channel, uint32_t value) {
+  uint32_t raw_val = MAX(MIN(value * (1U << 8U) / 3300U, (1U << 8U)), 0U);  
+  switch(channel) {
+    case 1:
+      register_set(&dac->DHR8R1, raw_val, 0xFFU);
+      break;
+    case 2:
+      register_set(&dac->DHR8R2, raw_val, 0xFFU);
+      break;
+    default:
+      break;
+  }
+}
diff --git a/board/stm32h7/lli2c.h b/board/stm32h7/lli2c.h
@@ -0,0 +1,153 @@
+
+// TODO: this driver relies heavily on polling,
+// if we want it to be more async, we should use interrupts
+
+#define I2C_TIMEOUT_US 100000U
+
+// cppcheck-suppress misra-c2012-2.7; not sure why it triggers here?
+bool i2c_status_wait(volatile uint32_t *reg, uint32_t mask, uint32_t val) {
+  uint32_t start_time = microsecond_timer_get();
+  while(((*reg & mask) != val) && (get_ts_elapsed(microsecond_timer_get(), start_time) < I2C_TIMEOUT_US));
+  return ((*reg & mask) == val);
+}
+
+bool i2c_write_reg(I2C_TypeDef *I2C, uint8_t addr, uint8_t reg, uint8_t value) {
+  // Setup transfer and send START + addr
+  bool ret = false;
+  for(uint32_t i=0U; i<10U; i++) {
+    register_clear_bits(&I2C->CR2, I2C_CR2_ADD10);
+    I2C->CR2 = ((addr << 1U) & I2C_CR2_SADD_Msk);
+    register_clear_bits(&I2C->CR2, I2C_CR2_RD_WRN);
+    register_set_bits(&I2C->CR2, I2C_CR2_AUTOEND);
+    I2C->CR2 |= (2 << I2C_CR2_NBYTES_Pos);
+
+    I2C->CR2 |= I2C_CR2_START;
+    if(!i2c_status_wait(&I2C->CR2, I2C_CR2_START, 0U)) {
+      continue;
+    }
+
+    // check if we lost arbitration
+    if ((I2C->ISR & I2C_ISR_ARLO) != 0U) {
+      register_set_bits(&I2C->ICR, I2C_ICR_ARLOCF);
+    } else {
+      ret = true;
+      break;
+    }
+  }
+
+  if (!ret) {
+    goto end;
+  }
+
+  // Send data
+  ret = i2c_status_wait(&I2C->ISR, I2C_ISR_TXIS, I2C_ISR_TXIS);
+  if(!ret) {
+    goto end;
+  }
+  I2C->TXDR = reg;
+
+  ret = i2c_status_wait(&I2C->ISR, I2C_ISR_TXIS, I2C_ISR_TXIS);
+  if(!ret) {
+    goto end;
+  }
+  I2C->TXDR = value;
+
+end:
+  return ret;
+}
+
+bool i2c_read_reg(I2C_TypeDef *I2C, uint8_t addr, uint8_t reg, uint8_t *value) {
+  // Setup transfer and send START + addr
+  bool ret = false;
+  for(uint32_t i=0U; i<10U; i++) {
+    register_clear_bits(&I2C->CR2, I2C_CR2_ADD10);
+    I2C->CR2 = ((addr << 1U) & I2C_CR2_SADD_Msk);
+    register_clear_bits(&I2C->CR2, I2C_CR2_RD_WRN);
+    register_clear_bits(&I2C->CR2, I2C_CR2_AUTOEND);
+    I2C->CR2 |= (1 << I2C_CR2_NBYTES_Pos);
+
+    I2C->CR2 |= I2C_CR2_START;
+    if(!i2c_status_wait(&I2C->CR2, I2C_CR2_START, 0U)) {
+      continue;
+    }
+
+    // check if we lost arbitration
+    if ((I2C->ISR & I2C_ISR_ARLO) != 0U) {
+      register_set_bits(&I2C->ICR, I2C_ICR_ARLOCF);
+    } else {
+      ret = true;
+      break;
+    }
+  }
+
+  if (!ret) {
+    goto end;
+  }
+
+  // Send data
+  ret = i2c_status_wait(&I2C->ISR, I2C_ISR_TXIS, I2C_ISR_TXIS);
+  if(!ret) {
+    goto end;
+  }
+  I2C->TXDR = reg;
+
+  // Restart
+  I2C->CR2 = (((addr << 1) | 0x1U) & I2C_CR2_SADD_Msk) | (1U << I2C_CR2_NBYTES_Pos) | I2C_CR2_RD_WRN | I2C_CR2_START;
+  ret = i2c_status_wait(&I2C->CR2, I2C_CR2_START, 0U);
+  if(!ret) {
+    goto end;
+  }
+
+  // check if we lost arbitration
+  if ((I2C->ISR & I2C_ISR_ARLO) != 0U) {
+    register_set_bits(&I2C->ICR, I2C_ICR_ARLOCF);
+    ret = false;
+    goto end;
+  }
+
+  // Read data
+  ret = i2c_status_wait(&I2C->ISR, I2C_ISR_RXNE, I2C_ISR_RXNE);
+  if(!ret) {
+    goto end;
+  }
+  *value = I2C->RXDR;
+
+  // Stop
+  I2C->CR2 |= I2C_CR2_STOP;
+
+end:
+  return ret;
+}
+
+bool i2c_set_reg_bits(I2C_TypeDef *I2C, uint8_t addr, uint8_t reg, uint8_t bits) {
+  uint8_t value;
+  bool ret = i2c_read_reg(I2C, addr, reg, &value);
+  if(ret) {
+    ret = i2c_write_reg(I2C, addr, reg, value | bits);
+  }
+  return ret;
+}
+
+bool i2c_clear_reg_bits(I2C_TypeDef *I2C, uint8_t addr, uint8_t reg, uint8_t bits) {
+  uint8_t value;
+  bool ret = i2c_read_reg(I2C, addr, reg, &value);
+  if(ret) {
+    ret = i2c_write_reg(I2C, addr, reg, value & (uint8_t) (~bits));
+  }
+  return ret;
+}
+
+bool i2c_set_reg_mask(I2C_TypeDef *I2C, uint8_t addr, uint8_t reg, uint8_t value, uint8_t mask) {
+  uint8_t old_value;
+  bool ret = i2c_read_reg(I2C, addr, reg, &old_value);
+  if(ret) {
+    ret = i2c_write_reg(I2C, addr, reg, (old_value & (uint8_t) (~mask)) | (value & mask));
+  }
+  return ret;
+}
+
+void i2c_init(I2C_TypeDef *I2C) {
+  // 100kHz clock speed
+  I2C->TIMINGR = 0x107075B0;
+  I2C->CR1 = I2C_CR1_PE;
+}
diff --git a/board/stm32h7/peripherals.h b/board/stm32h7/peripherals.h
@@ -100,13 +100,17 @@ void peripherals_init(void) {
   RCC->AHB4ENR |= RCC_AHB4ENR_GPIOGEN;
 
   RCC->APB2ENR |= RCC_APB2ENR_SPI4EN;  // SPI
+  RCC->AHB1ENR |= RCC_AHB1ENR_DMA1EN;  // DAC DMA
   RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN;  // SPI DMA
   RCC->APB1LENR |= RCC_APB1LENR_TIM2EN;  // main counter
   RCC->APB1LENR |= RCC_APB1LENR_TIM3EN;  // fan pwm
   RCC->APB1LENR |= RCC_APB1LENR_TIM6EN;  // interrupt timer
+  RCC->APB1LENR |= RCC_APB1LENR_TIM7EN;  // DMA trigger timer
   RCC->APB1LENR |= RCC_APB1LENR_UART7EN;  // SOM uart
+  RCC->APB1LENR |= RCC_APB1LENR_DAC12EN; // DAC
   RCC->APB2ENR |= RCC_APB2ENR_TIM8EN;  // clock source timer
   RCC->APB1LENR |= RCC_APB1LENR_TIM12EN;  // slow loop
+  RCC->APB1LENR |= RCC_APB1LENR_I2C5EN;  // codec I2C
 
   RCC->APB1HENR |= RCC_APB1HENR_FDCANEN; // FDCAN core enable
   RCC->AHB1ENR |= RCC_AHB1ENR_ADC12EN; // Enable ADC clocks