2 files changed, 38 insertions, 7 deletions

diff --git a/embassy-stm32/src/tsc/enums.rs b/embassy-stm32/src/tsc/enums.rs
index 56df4173a..bc8e9d2f5 100644
--- a/embassy-stm32/src/tsc/enums.rs
+++ b/embassy-stm32/src/tsc/enums.rs
@@ -90,9 +90,28 @@ impl Into<u32> for TscIOPin {
     }
 }
 
+/// Spread Spectrum Deviation
+#[derive(Copy, Clone)]
+pub struct SSDeviation(u8);
+impl SSDeviation {
+    /// Create new deviation value, acceptable inputs are 1-128
+    pub fn new(val: u8) -> Result<Self, ()> {
+        if val == 0 || val > 128 {
+            return Err(());
+        }
+        Ok(Self(val - 1))
+    }
+}
+
+impl Into<u8> for SSDeviation {
+    fn into(self) -> u8 {
+        self.0
+    }
+}
+
 /// Charge transfer pulse cycles
 #[allow(missing_docs)]
-#[derive(Copy, Clone)]
+#[derive(Copy, Clone, PartialEq)]
 pub enum ChargeTransferPulseCycle {
     _1,
     _2,
@@ -137,7 +156,7 @@ impl Into<u8> for ChargeTransferPulseCycle {
 
 /// Prescaler divider
 #[allow(missing_docs)]
-#[derive(Copy, Clone)]
+#[derive(Copy, Clone, PartialEq)]
 pub enum PGPrescalerDivider {
     _1,
     _2,
diff --git a/embassy-stm32/src/tsc/mod.rs b/embassy-stm32/src/tsc/mod.rs
index 91cf7187a..8da0e87ab 100644
--- a/embassy-stm32/src/tsc/mod.rs
+++ b/embassy-stm32/src/tsc/mod.rs
@@ -6,7 +6,6 @@
 pub mod enums;
 
 use crate::gpio::AnyPin;
-use crate::pac::tsc::regs;
 use crate::{pac::tsc::Tsc as Regs, rcc::RccPeripheral};
 use crate::{peripherals, Peripheral};
 use embassy_hal_internal::{into_ref, PeripheralRef};
@@ -98,7 +97,7 @@ pub struct Config {
     /// Enable/disable of spread spectrum feature
     pub spread_spectrum: bool,
     /// Adds variable number of periods of the SS clk to pulse high state
-    pub spread_spectrum_deviation: u8,
+    pub spread_spectrum_deviation: SSDeviation,
     /// Selects AHB clock divider used to generate SS clk
     pub spread_spectrum_prescaler: bool,
     /// Selects AHB clock divider used to generate pulse generator clk
@@ -127,7 +126,7 @@ impl Default for Config {
             ct_pulse_high_length: ChargeTransferPulseCycle::_1,
             ct_pulse_low_length: ChargeTransferPulseCycle::_1,
             spread_spectrum: false,
-            spread_spectrum_deviation: 0,
+            spread_spectrum_deviation: SSDeviation::new(0).unwrap(),
             spread_spectrum_prescaler: false,
             pulse_generator_prescaler: PGPrescalerDivider::_1,
             max_count_value: MaxCount::_255,
@@ -255,9 +254,22 @@ impl<'d, T: Instance> Tsc<'d, T> {
             w.set_ctph(config.ct_pulse_high_length.into());
             w.set_ctpl(config.ct_pulse_low_length.into());
             w.set_sse(config.spread_spectrum);
-            w.set_ssd(config.spread_spectrum_deviation);
+            // Prevent invalid configuration for pulse generator prescaler
+            if config.ct_pulse_low_length == ChargeTransferPulseCycle::_1
+                && (config.pulse_generator_prescaler == PGPrescalerDivider::_1
+                    || config.pulse_generator_prescaler == PGPrescalerDivider::_2)
+            {
+                w.set_pgpsc(PGPrescalerDivider::_4.into());
+            } else if config.ct_pulse_low_length == ChargeTransferPulseCycle::_2
+                && config.pulse_generator_prescaler == PGPrescalerDivider::_1
+            {
+                w.set_pgpsc(PGPrescalerDivider::_2.into());
+            } else {
+                w.set_pgpsc(config.pulse_generator_prescaler.into());
+            }
+            w.set_ssd(config.spread_spectrum_deviation.into());
             w.set_sspsc(config.spread_spectrum_prescaler);
-            w.set_pgpsc(config.pulse_generator_prescaler.into());
+
             w.set_mcv(config.max_count_value.into());
             w.set_syncpol(config.synchro_pin_polarity);
             w.set_am(config.acquisition_mode);