Add ROM function caching

Add a feature to enable automatic caching of the result of ROM table
function lookups.

rp2040-hal/Cargo.toml

@@ -53,3 +53,4 @@ rt = ["rp2040-pac/rt"]
 #
 # embassy-traits = ["embassy_traits", "futures"]
 alloc = []
+rom-func-cache = []

rp2040-hal/src/rom_data.rs

@@ -58,6 +58,7 @@ macro_rules! declare_rom_function {
         #[doc = r"` ROM function."]
         pub mod $name {
             /// Retrieve a function pointer.
+            #[cfg(not(feature = "rom-func-cache"))]
             pub fn ptr() -> extern "C" fn( $($argname: $ty),* ) -> $ret {
                 let p: *const u32 = $lookup;
                 unsafe {
@@ -65,6 +66,32 @@ macro_rules! declare_rom_function {
                     func
                 }
             }
+
+            /// Retrieve a function pointer.
+            #[cfg(feature = "rom-func-cache")]
+            pub fn ptr() -> extern "C" fn( $($argname: $ty),* ) -> $ret {
+                use core::sync::atomic::{AtomicU16, Ordering};
+
+                // All pointers in the ROM fit in 16 bits, so we don't need a
+                // full width word to store the cached value.
+                static CACHED_PTR: AtomicU16 = AtomicU16::new(0);
+                // This is safe because the lookup will always resolve
+                // to the same value.  So even if an interrupt or another
+                // core starts at the same time, it just repeats some
+                // work and eventually writes back the correct value.
+                let p: *const u32 = match CACHED_PTR.load(Ordering::Relaxed) {
+                    0 => {
+                        let raw: *const u32 = $lookup;
+                        CACHED_PTR.store(raw as u16, Ordering::Relaxed);
+                        raw
+                    },
+                    val => val as *const u32,
+                };
+                unsafe {
+                    let func : extern "C" fn( $($argname: $ty),* ) -> $ret = core::mem::transmute(p);
+                    func
+                }
+            }
         }
 
         $(#[$outer])*
@@ -83,6 +110,7 @@ macro_rules! declare_rom_function {
         #[doc = r"` ROM function."]
         pub mod $name {
             /// Retrieve a function pointer.
+            #[cfg(not(feature = "rom-func-cache"))]
             pub fn ptr() -> unsafe extern "C" fn( $($argname: $ty),* ) -> $ret {
                 let p: *const u32 = $lookup;
                 unsafe {
@@ -90,6 +118,32 @@ macro_rules! declare_rom_function {
                     func
                 }
             }
+
+            /// Retrieve a function pointer.
+            #[cfg(feature = "rom-func-cache")]
+            pub fn ptr() -> unsafe extern "C" fn( $($argname: $ty),* ) -> $ret {
+                use core::sync::atomic::{AtomicU16, Ordering};
+
+                // All pointers in the ROM fit in 16 bits, so we don't need a
+                // full width word to store the cached value.
+                static CACHED_PTR: AtomicU16 = AtomicU16::new(0);
+                // This is safe because the lookup will always resolve
+                // to the same value.  So even if an interrupt or another
+                // core starts at the same time, it just repeats some
+                // work and eventually writes back the correct value.
+                let p: *const u32 = match CACHED_PTR.load(Ordering::Relaxed) {
+                    0 => {
+                        let raw: *const u32 = $lookup;
+                        CACHED_PTR.store(raw as u16, Ordering::Relaxed);
+                        raw
+                    },
+                    val => val as *const u32,
+                };
+                unsafe {
+                    let func : unsafe extern "C" fn( $($argname: $ty),* ) -> $ret = core::mem::transmute(p);
+                    func
+                }
+            }
         }
 
         $(#[$outer])*