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@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 ;