//! Motor GPIO-low interlock, called before all fallible initialization.

use crate::MotorPins;
use stm32f7::stm32f722;

pub(crate) fn inhibit(
    rcc: &stm32f722::rcc::RegisterBlock,
    port: &stm32f722::gpiob::RegisterBlock,
    pins: MotorPins,
) {
    rcc.ahb1enr().modify(|_, write| write.gpioben().set_bit());
    // RM0431 section 5.2.12 requires two peripheral clock cycles before the
    // first access. The volatile enable-register read supplies the standard
    // synchronization delay and prevents the GPIO write from being reordered
    // ahead of the clock enable.
    let _ = rcc.ahb1enr().read().gpioben().bit_is_set();
    port.bsrr().write(|write| {
        let write = write
            .br0()
            .set_bit()
            .br1()
            .set_bit()
            .br4()
            .set_bit()
            .br5()
            .set_bit();
        if pins == MotorPins::Six {
            write.br6().set_bit().br7().set_bit()
        } else {
            write
        }
    });
    port.otyper().modify(|_, write| {
        let write = write
            .ot0()
            .push_pull()
            .ot1()
            .push_pull()
            .ot4()
            .push_pull()
            .ot5()
            .push_pull();
        if pins == MotorPins::Six {
            write.ot6().push_pull().ot7().push_pull()
        } else {
            write
        }
    });
    port.ospeedr().modify(|_, write| {
        let write = write
            .ospeedr0()
            .low_speed()
            .ospeedr1()
            .low_speed()
            .ospeedr4()
            .low_speed()
            .ospeedr5()
            .low_speed();
        if pins == MotorPins::Six {
            write.ospeedr6().low_speed().ospeedr7().low_speed()
        } else {
            write
        }
    });
    port.pupdr().modify(|_, write| {
        let write = write
            .pupdr0()
            .floating()
            .pupdr1()
            .floating()
            .pupdr4()
            .floating()
            .pupdr5()
            .floating();
        if pins == MotorPins::Six {
            write.pupdr6().floating().pupdr7().floating()
        } else {
            write
        }
    });
    port.moder().modify(|_, write| {
        let write = write
            .moder0()
            .output()
            .moder1()
            .output()
            .moder4()
            .output()
            .moder5()
            .output();
        if pins == MotorPins::Six {
            write.moder6().output().moder7().output()
        } else {
            write
        }
    });
}

#[cfg(test)]
mod tests {
    use super::*;
    use core::mem::MaybeUninit;

    #[test]
    fn inhibits_only_board_motor_pins() {
        for (pins, pin_mask, field_mask, output_mode) in [
            (MotorPins::Four, 0x0033, 0x0f0f, 0x0505),
            (MotorPins::Six, 0x00f3, 0xff0f, 0x5505),
        ] {
            // SAFETY: these PAC blocks contain u32 volatile cells and reserved
            // bytes. Zero is valid, and this test owns their ordinary RAM storage.
            let (rcc, port) = unsafe {
                (
                    MaybeUninit::<stm32f722::rcc::RegisterBlock>::zeroed().assume_init(),
                    MaybeUninit::<stm32f722::gpiob::RegisterBlock>::zeroed().assume_init(),
                )
            };
            // SAFETY: writes initialize only the test's local register storage.
            unsafe {
                rcc.ahb1enr().write(|write| write.bits(1));
                port.moder().write(|write| write.bits(u32::MAX));
                port.otyper().write(|write| write.bits(0xffff));
                port.ospeedr().write(|write| write.bits(u32::MAX));
                port.pupdr().write(|write| write.bits(0xaaaa_aaaa));
            }
            inhibit(&rcc, &port, pins);
            assert_eq!(rcc.ahb1enr().read().bits(), 3);
            // SAFETY: this reads the local backing cell, not a hardware write-only register.
            let reset = unsafe { port.bsrr().as_ptr().read_volatile() };
            assert_eq!(reset, pin_mask << 16);
            assert_eq!(port.moder().read().bits(), !field_mask | output_mode);
            assert_eq!(port.otyper().read().bits(), 0xffff & !pin_mask);
            assert_eq!(port.ospeedr().read().bits(), !field_mask);
            assert_eq!(port.pupdr().read().bits(), 0xaaaa_aaaa & !field_mask);
        }
    }
}