[OpenBIOS] [PATCH 2/2] sbus: add probe for display device

[Mark Cave-Ayland]

If we are unable to find valid FCode at the probe address then we fallback to
the internal TCX driver in order to preserve existing behaviour.

Signed-off-by: Mark Cave-Ayland <mark.cave-ayland at ilande.co.uk>
---
 openbios-devel/drivers/sbus.c  |   30 ++++++++++++++++++++++++++++--
 openbios-devel/drivers/sbus.fs |   27 ++++++++++++++++++---------
 2 files changed, 46 insertions(+), 11 deletions(-)

diff --git a/openbios-devel/drivers/sbus.c b/openbios-devel/drivers/sbus.c
index 6ef6bda..128f3a8 100644
--- a/openbios-devel/drivers/sbus.c
+++ b/openbios-devel/drivers/sbus.c
@@ -141,8 +141,34 @@ uint16_t graphic_depth;
 static void
 ob_tcx_init(unsigned int slot, const char *path)
 {
-    push_str(path);
-    fword("find-device");
+    phandle_t ph = 0;
+    int found = 0;
+    char buf[6];
+
+    while ((ph = dt_iterate_type(ph, "display"))) {
+        found = -1;
+    }
+
+    if (!found) {
+        printk("No display device located during SBus probe - falling back to internal TCX driver\n");
+
+        /* Make the sbus node the current instance and active package for probing */
+        feval("active-package my-self");
+        push_str("/iommu/sbus");
+        feval("2dup find-device open-dev to my-self");
+
+        fword("new-device");
+        PUSH(0);
+        PUSH(0);
+        snprintf(buf, 6, "%d,0", slot);
+        push_str(buf);
+        fword("set-args");
+        feval("['] tcx-driver-fcode 2 cells + 1 byte-load");
+        fword("finish-device");
+
+        /* Restore */
+        feval("to my-self active-package!");
+    }
 }
 
 static void
diff --git a/openbios-devel/drivers/sbus.fs b/openbios-devel/drivers/sbus.fs
index fa8c95c..22f3f72 100644
--- a/openbios-devel/drivers/sbus.fs
+++ b/openbios-devel/drivers/sbus.fs
@@ -64,15 +64,24 @@
 \ -------------------------------------------------------------------------
 
 : probe-self-sbus ( arg-adr arg-len reg-adr reg-len fcode-adr fcode-len -- )
-  2drop
-  new-device
-  set-args
 
-  \ Note: this is currently hardcoded to TCX for testing as we don't have
-  \ cpeek (yet). Once cpeek is in place, adapting this to probe any slot
-  \ will be faily easy.
-  " tcx-driver-fcode" $find drop 2 cells +
-  1 byte-load
+  ['] decode-unit-sbus catch if
+    2drop 2drop 2drop 2drop
+    exit
+  then
+
+  h# 10000 map-in-sbus
+
+  dup c@
+  dup h# f1 = swap h# fd = or if
+    new-device
+    >r set-args r>
+    dup 1 byte-load
+    finish-device
+  else
+    nip nip nip nip
+    ." Invalid FCode start byte" cr
+  then
 
-  finish-device
+  h# 10000 map-out-sbus
 ;
-- 
1.7.10.4