[OpenBIOS] [PATCH 3/3] SPARC32: handle data faults for peek/poke

[Bob Breuer]

Signed-off-by: Bob Breuer <breuerr at mc.net>
---
 openbios-devel/arch/sparc32/init.fs |   30 ++++++++++++++++++++++++++++++
 openbios-devel/arch/sparc32/lib.c   |   21 +++++++++++++++++++++
 2 files changed, 51 insertions(+)

diff --git a/openbios-devel/arch/sparc32/init.fs b/openbios-devel/arch/sparc32/init.fs
index e2cd571..814c720 100644
--- a/openbios-devel/arch/sparc32/init.fs
+++ b/openbios-devel/arch/sparc32/init.fs
@@ -46,6 +46,36 @@ device-end
   0
   ;
 
+\ (peek) and (poke) implementation
+defer sfsr@
+defer ignore-dfault
+
+:noname
+  \ ( addr xt -- false | value true )
+  sfsr@ drop            \ Clear any existing MMU fault status
+
+  -1 ignore-dfault !    \ Disable data fault trap
+  execute
+  0 ignore-dfault !     \ Enable data fault trap
+
+  sfsr@ 0= if
+    true
+  else
+    drop false          \ Failed, drop the read value
+  then
+; to (peek)
+
+:noname
+  \ ( value addr xt -- okay? )
+  sfsr@ drop            \ Clear any existing MMU fault status
+
+  -1 ignore-dfault !    \ Disable data fault trap
+  execute
+  0 ignore-dfault !     \ Enable data fault trap
+
+  sfsr@ 0=              \ true if no fault
+; to (poke)
+
 \ Load TCX FCode driver blob
 [IFDEF] CONFIG_DRIVER_SBUS
   -1 value tcx-driver-fcode
diff --git a/openbios-devel/arch/sparc32/lib.c b/openbios-devel/arch/sparc32/lib.c
index 81d30ad..a871b69 100644
--- a/openbios-devel/arch/sparc32/lib.c
+++ b/openbios-devel/arch/sparc32/lib.c
@@ -302,6 +302,22 @@ void obp_dumb_memfree(__attribute__((unused))char *va,
     DPRINTF("obp_dumb_memfree 0x%p (size %d)\n", va, sz);
 }
 
+extern unsigned int ignore_dfault;
+
+/* Data fault handling routines */
+
+/* ( -- reg ) */
+static void srmmu_get_sfsr(void)
+{
+    PUSH(srmmu_get_fstatus());
+}
+
+/* ( -- addr ) */
+static void ignore_dfault_addr(void)
+{
+    PUSH(pointer2cell(&ignore_dfault));
+}
+
 void
 ob_init_mmu(void)
 {
@@ -360,6 +376,11 @@ ob_init_mmu(void)
     bind_func("pgmap@", pgmap_fetch);
     bind_func("pgmap!", pgmap_store);
     bind_func("map-pages", ob_map_pages);
+
+    PUSH_xt( bind_noname_func(srmmu_get_sfsr) );
+    feval("to sfsr@");
+    PUSH_xt( bind_noname_func(ignore_dfault_addr) );
+    feval("to ignore-dfault");
 }
 
 /*
-- 
1.7.10.4