Make hardware divisor interrupt safe

Copy the pico-sdk logic for save and restore of the hardware divider
for making it interrupt safe.

rp2040-hal/src/sio.rs

@@ -165,12 +165,61 @@ impl SioFifo {
     }
 }
 
+fn save_divider<F, R>(f: F) -> R
+where
+    F: FnOnce(&pac::sio::RegisterBlock) -> R,
+{
+    let sio = unsafe { &(*pac::SIO::ptr()) };
+    if !sio.div_csr.read().dirty().bit() {
+        // Not dirty, so nothing is waiting for the calculation.  So we can just
+        // issue it directly without a save/restore.
+        f(sio)
+    } else {
+        // Since we can't save the signed-ness of the calculation, we have to make
+        // sure that there's at least an 8 cycle delay before we read the result.
+        // The Pico SDK ensures this by using a 6 cycle push and two 1 cycle reads.
+        // Since we can't be sure the Rust implementation will optimize to the same,
+        // just use an explicit wait.
+        while !sio.div_csr.read().ready().bit() {}
+
+        // Read the quotient last, since that's what clears the dirty flag
+        let dividend = sio.div_udividend.read().bits();
+        let divisor = sio.div_udivisor.read().bits();
+        let remainder = sio.div_remainder.read().bits();
+        let quotient = sio.div_quotient.read().bits();
+
+        // If we get interrupted here (before a write sets the DIRTY flag) its fine, since
+        // we have the full state, so the interruptor doesn't have to restore it.  Once the
+        // write happens and the DIRTY flag is set, the interruptor becomes responsible for
+        // restoring our state.
+        let result = f(sio);
+
+        // If we are interrupted here, then the interruptor will start an incorrect calculation
+        // using a wrong divisor, but we'll restore the divisor and result ourselves correctly.
+        // This sets DIRTY, so any interruptor will save the state.
+        sio.div_udividend.write(|w| unsafe { w.bits(dividend) });
+        // If we are interrupted here, the the interruptor may start the calculation using
+        // incorrectly signed inputs, but we'll restore the result ourselves.
+        // This sets DIRTY, so any interruptor will save the state.
+        sio.div_udivisor.write(|w| unsafe { w.bits(divisor) });
+        // If we are interrupted here, the interruptor will have restored everything but the
+        // quotient may be wrongly signed.  If the calculation started by the above writes is
+        // still ongoing it is stopped, so it won't replace the result we're restoring.
+        // DIRTY and READY set, but only DIRTY matters to make the interruptor save the state.
+        sio.div_remainder.write(|w| unsafe { w.bits(remainder) });
+        // State fully restored after the quotient write.  This sets both DIRTY and READY, so
+        // whatever we may have interrupted can read the result.
+        sio.div_quotient.write(|w| unsafe { w.bits(quotient) });
+
+        result
+    }
+}
+
 impl HwDivider {
     /// Perform hardware unsigned divide/modulo operation
     pub fn unsigned(&self, dividend: u32, divisor: u32) -> DivResult<u32> {
-        let sio = unsafe { &(*pac::SIO::ptr()) };
+        save_divider(|sio| {
             sio.div_udividend.write(|w| unsafe { w.bits(dividend) });
-
             sio.div_udivisor.write(|w| unsafe { w.bits(divisor) });
 
             cortex_m::asm::delay(8);
@@ -183,14 +232,14 @@ impl HwDivider {
                 remainder,
                 quotient,
             }
+        })
     }
 
     /// Perform hardware signed divide/modulo operation
     pub fn signed(&self, dividend: i32, divisor: i32) -> DivResult<i32> {
-        let sio = unsafe { &(*pac::SIO::ptr()) };
+        save_divider(|sio| {
             sio.div_sdividend
                 .write(|w| unsafe { w.bits(dividend as u32) });
-
             sio.div_sdivisor
                 .write(|w| unsafe { w.bits(divisor as u32) });
 
@@ -204,6 +253,7 @@ impl HwDivider {
                 remainder,
                 quotient,
             }
+        })
     }
 }