kaptan/platforms/stm32f7/src/actuator/tim3_pwm.rs

use core::sync::atomic::{AtomicU16, Ordering};

use kaptan_actuator::pwm::{ArmSequence, Config, FrameValue, PulseWidth};
use stm32f7::stm32f722;

const APB1_TIMER_CLOCK_HZ: u32 = 108_000_000;
const TIMER_TICK_HZ: u32 = 1_000_000;
const PWM_CHANNEL_ENABLE: u32 = 0x1111;
const PWM_MODE_1_WITH_PRELOAD: u32 = 0x6868;
const DEFAULT_MINIMUM_PULSE: PulseWidth = match PulseWidth::new(1_000) {
    Some(pulse) => pulse,
    None => panic!("valid PWM pulse"),
};
const DEFAULT_ARMED_IDLE_PULSE: PulseWidth = match PulseWidth::new(1_000) {
    Some(pulse) => pulse,
    None => panic!("valid PWM pulse"),
};

static ARM_SEQUENCE: ArmSequence = ArmSequence::new(DEFAULT_ARMED_IDLE_PULSE, 0);
static MINIMUM_PULSE: AtomicU16 = AtomicU16::new(DEFAULT_MINIMUM_PULSE.microseconds());

pub(super) struct Tim3Pwm {
    timer: stm32f722::TIM3,
    minimum_pulse: PulseWidth,
}

impl Tim3Pwm {
    pub(super) fn new(rcc: &stm32f722::RCC, timer: stm32f722::TIM3, config: Config) -> Self {
        let period_us = match config.frame_rate().period_microseconds() {
            Some(period) => period,
            None => unreachable!(),
        };
        ARM_SEQUENCE.configure_minimum_frames(
            config
                .frame_rate()
                .frames_for_millis(config.arming_duration().milliseconds()),
        );
        ARM_SEQUENCE.configure_armed_idle(config.armed_idle_pulse());
        MINIMUM_PULSE.store(config.minimum_pulse().microseconds(), Ordering::Release);

        rcc.ahb1enr().modify(|_, write| write.gpioben().set_bit());
        rcc.apb1rstr().modify(|_, write| write.tim3rst().set_bit());
        rcc.apb1rstr()
            .modify(|_, write| write.tim3rst().clear_bit());
        rcc.apb1enr().modify(|_, write| write.tim3en().set_bit());

        configure_gpio_alternate_function();
        timer
            .cr1()
            .write(|write| write.cen().clear_bit().arpe().enabled());
        timer.psc().write(|write| {
            // SAFETY: the fixed APB1 clock divides exactly to the documented 1 MHz timer tick.
            unsafe {
                write
                    .psc()
                    .bits((APB1_TIMER_CLOCK_HZ / TIMER_TICK_HZ - 1) as u16)
            }
        });
        timer.arr().write(|write| {
            // SAFETY: `frame_rate` guarantees a non-zero period fitting TIM3's ARR register.
            unsafe { write.bits(u32::from(period_us - 1)) }
        });
        timer.ccmr1_output().write(|write| {
            // SAFETY: this is the documented PWM mode 1 plus preload encoding for channels 1 and 2.
            unsafe { write.bits(PWM_MODE_1_WITH_PRELOAD) }
        });
        timer.ccmr2_output().write(|write| {
            // SAFETY: this is the documented PWM mode 1 plus preload encoding for channels 3 and 4.
            unsafe { write.bits(PWM_MODE_1_WITH_PRELOAD) }
        });
        write_pulse(&timer, config.minimum_pulse());
        timer.ccer().write(|write| {
            // SAFETY: this enables only the four TIM3 output channels configured above.
            unsafe { write.bits(PWM_CHANNEL_ENABLE) }
        });
        timer.egr().write(|write| write.ug().set_bit());
        timer.sr().modify(|_, write| write.uif().clear());
        timer.dier().write(|write| write.uie().enabled());
        timer.cr1().modify(|_, write| write.cen().set_bit());

        Self {
            timer,
            minimum_pulse: config.minimum_pulse(),
        }
    }

    pub(super) fn arm(&mut self) {
        ARM_SEQUENCE.arm();
    }

    pub(super) fn disarm(&mut self) {
        ARM_SEQUENCE.disarm();
        write_pulse(&self.timer, self.minimum_pulse);
    }

    pub(super) fn is_armed(&self) -> bool {
        ARM_SEQUENCE.is_armed()
    }

    pub(super) fn on_tim3_update() {
        // SAFETY: TIM3 is exclusively owned by the platform motor resource; RTIC locks that
        // resource across the service task and this update handler.
        let timer = unsafe { &*stm32f722::TIM3::ptr() };
        timer.sr().modify(|_, write| write.uif().clear());
        let pulse = match ARM_SEQUENCE.next_frame() {
            Some(FrameValue::ArmedIdle(pulse)) => pulse,
            Some(FrameValue::Minimum) | None => configured_minimum_pulse(),
        };
        write_pulse(timer, pulse);
    }
}

fn configured_minimum_pulse() -> PulseWidth {
    match PulseWidth::new(MINIMUM_PULSE.load(Ordering::Acquire)) {
        Some(pulse) => pulse,
        None => unreachable!(),
    }
}

fn write_pulse(timer: &stm32f722::tim3::RegisterBlock, pulse: PulseWidth) {
    let ticks = u32::from(pulse.microseconds());
    timer.ccr1().write(|write| {
        // SAFETY: `PulseWidth` bounds ticks to the configured PWM frame interval.
        unsafe { write.bits(ticks) }
    });
    timer.ccr2().write(|write| {
        // SAFETY: `PulseWidth` bounds ticks to the configured PWM frame interval.
        unsafe { write.bits(ticks) }
    });
    timer.ccr3().write(|write| {
        // SAFETY: `PulseWidth` bounds ticks to the configured PWM frame interval.
        unsafe { write.bits(ticks) }
    });
    timer.ccr4().write(|write| {
        // SAFETY: `PulseWidth` bounds ticks to the configured PWM frame interval.
        unsafe { write.bits(ticks) }
    });
}

fn port() -> &'static stm32f722::gpiob::RegisterBlock {
    // SAFETY: PB0/PB1/PB4/PB5 are exclusively claimed by the selected TIM3 motor route; board
    // code never receives GPIOB and RTIC serializes platform writes to these registers.
    unsafe { &*stm32f722::GPIOB::ptr() }
}

fn configure_gpio_alternate_function() {
    let port = port();
    port.afrl().modify(|_, write| {
        // SAFETY: AF2 selects TIM3 on these four reserved GPIOB fields, leaving all other pins intact.
        unsafe {
            write
                .afrl0()
                .bits(2)
                .afrl1()
                .bits(2)
                .afrl4()
                .bits(2)
                .afrl5()
                .bits(2)
        }
    });
    port.moder().modify(|_, write| {
        write
            .moder0()
            .alternate()
            .moder1()
            .alternate()
            .moder4()
            .alternate()
            .moder5()
            .alternate()
    });
    port.otyper().modify(|_, write| {
        write
            .ot0()
            .push_pull()
            .ot1()
            .push_pull()
            .ot4()
            .push_pull()
            .ot5()
            .push_pull()
    });
    port.ospeedr().modify(|_, write| {
        write
            .ospeedr0()
            .high_speed()
            .ospeedr1()
            .high_speed()
            .ospeedr4()
            .high_speed()
            .ospeedr5()
            .high_speed()
    });
    port.pupdr().modify(|_, write| {
        write
            .pupdr0()
            .floating()
            .pupdr1()
            .floating()
            .pupdr4()
            .floating()
            .pupdr5()
            .floating()
    });
}