From 98fd6c1724e3fbea60940d67e57567e2e98df598 Mon Sep 17 00:00:00 2001
From: Derek Hageman <hageman@inthat.cloud>
Date: Sat, 29 Jan 2022 17:46:45 -0700
Subject: [PATCH] Add ROM function caching

Add a feature to enable automatic caching of the result of ROM table
function lookups.
---
 rp2040-hal/Cargo.toml      |  1 +
 rp2040-hal/src/rom_data.rs | 54 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 55 insertions(+)

diff --git a/rp2040-hal/Cargo.toml b/rp2040-hal/Cargo.toml
index 2b323a5..342b76c 100644
--- a/rp2040-hal/Cargo.toml
+++ b/rp2040-hal/Cargo.toml
@@ -53,3 +53,4 @@ rt = ["rp2040-pac/rt"]
 #
 # embassy-traits = ["embassy_traits", "futures"]
 alloc = []
+rom-func-cache = []
diff --git a/rp2040-hal/src/rom_data.rs b/rp2040-hal/src/rom_data.rs
index 95981d8..d53d78f 100644
--- a/rp2040-hal/src/rom_data.rs
+++ b/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])*