From 716be4fc87412541fb1305c8592245a36104b584 Mon Sep 17 00:00:00 2001
From: Clyne Sullivan <clyne@bitgloo.com>
Date: Sat, 23 Jan 2021 10:43:40 -0500
Subject: [PATCH] boot at 32Ksps; fix 96Ksps

---
 ChibiOS_20.3.2/os/hal/ports/STM32/STM32H7xx/hal_lld.h | 3 ++-
 source/adc.cpp                                        | 9 +++++----
 source/adc.hpp                                        | 2 +-
 source/main.cpp                                       | 4 ++++
 4 files changed, 12 insertions(+), 6 deletions(-)

diff --git a/ChibiOS_20.3.2/os/hal/ports/STM32/STM32H7xx/hal_lld.h b/ChibiOS_20.3.2/os/hal/ports/STM32/STM32H7xx/hal_lld.h
index af36372..517ff16 100644
--- a/ChibiOS_20.3.2/os/hal/ports/STM32/STM32H7xx/hal_lld.h
+++ b/ChibiOS_20.3.2/os/hal/ports/STM32/STM32H7xx/hal_lld.h
@@ -377,6 +377,7 @@
 #define STM32_ODEN_DISABLED             0U
 #define STM32_ODEN_ENABLED              (SYSCFG_PWRCR_ODEN)
 
+#define STM32_VOS_SCALE0                0U
 #define STM32_VOS_SCALE3                (PWR_D3CR_VOS_0)
 #define STM32_VOS_SCALE2                (PWR_D3CR_VOS_1)
 #define STM32_VOS_SCALE1                (PWR_D3CR_VOS_1 | PWR_D3CR_VOS_0)
@@ -1404,7 +1405,7 @@
  * @name    Constants depending on VOS and ODEN setting
  * @{
  */
-#if STM32_VOS == STM32_VOS_SCALE1
+#if STM32_VOS == STM32_VOS_SCALE1 || STM32_VOS == STM32_VOS_SCALE0
 #define STM32_0WS_THRESHOLD         70000000U
 #define STM32_1WS_THRESHOLD         140000000U
 #define STM32_2WS_THRESHOLD         210000000U
diff --git a/source/adc.cpp b/source/adc.cpp
index ab06410..3bd6b39 100644
--- a/source/adc.cpp
+++ b/source/adc.cpp
@@ -18,7 +18,7 @@ const ADCConfig ADC::m_config = {
     .calibration = 0,
 };
 
-const ADCConversionGroup ADC::m_group_config = {
+ADCConversionGroup ADC::m_group_config = {
     .circular = true,
     .num_channels = 1,
     .end_cb = ADC::conversionCallback,
@@ -46,7 +46,7 @@ std::array<std::array<uint32_t, 2>, 6> ADC::m_rate_presets = {{
     {/* 20k */ 80,    8},
     {/* 32k */ 80,    5},
     {/* 48k */ 96,    4},
-    {/* 96k */ 96,    2}
+    {/* 96k */ 288,   10}
 }};
 
 adcsample_t *ADC::m_current_buffer = nullptr;
@@ -59,8 +59,6 @@ void ADC::begin()
 
     adcStart(m_driver, &m_config);
     adcSTM32EnableVREF(m_driver);
-
-    setRate(SClock::Rate::R32K);
 }
 
 void ADC::start(adcsample_t *buffer, size_t count, Operation operation)
@@ -101,6 +99,9 @@ void ADC::setRate(SClock::Rate rate)
     RCC->CR |= RCC_CR_PLL2ON;
     while ((RCC->CR & RCC_CR_PLL2RDY) != RCC_CR_PLL2RDY);
 
+    m_group_config.smpr[0] = rate != SClock::Rate::R96K ? ADC_SMPR1_SMP_AN5(ADC_SMPR_SMP_12P5)
+                                                        : ADC_SMPR1_SMP_AN5(ADC_SMPR_SMP_2P5);
+
     adcStart(m_driver, &m_config);
 }
 
diff --git a/source/adc.hpp b/source/adc.hpp
index 96fe506..488501c 100644
--- a/source/adc.hpp
+++ b/source/adc.hpp
@@ -34,7 +34,7 @@ private:
     static ADCDriver *m_driver;
 
     static const ADCConfig m_config;
-    static const ADCConversionGroup m_group_config;
+    static ADCConversionGroup m_group_config;
 
     static std::array<std::array<uint32_t, 2>, 6> m_rate_presets;
 
diff --git a/source/main.cpp b/source/main.cpp
index bb980d0..cc3138f 100644
--- a/source/main.cpp
+++ b/source/main.cpp
@@ -82,8 +82,12 @@ int main()
 
     ADC::begin();
     DAC::begin();
+    SClock::begin();
     USBSerial::begin();
 
+    SClock::setRate(SClock::Rate::R32K);
+    ADC::setRate(SClock::Rate::R32K);
+
     // Start the conversion manager thread
     chTMObjectInit(&conversion_time_measurement);
     chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1,