From b1c71512829e6488a68a50826b52a38842d1b216 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Markus=20Fr=C3=B6schle?= <mfro@mubf.de>
Date: Fri, 20 Nov 2015 21:45:59 +0000
Subject: [PATCH] fix crash on FireTOS

---
 BaS_gcc/tos/pci_test/sources/pci_test.c | 32 ++++++++++++++++++++++++-
 1 file changed, 31 insertions(+), 1 deletion(-)

diff --git a/BaS_gcc/tos/pci_test/sources/pci_test.c b/BaS_gcc/tos/pci_test/sources/pci_test.c
index c0e1bdd..0a9aa5d 100644
--- a/BaS_gcc/tos/pci_test/sources/pci_test.c
+++ b/BaS_gcc/tos/pci_test/sources/pci_test.c
@@ -58,9 +58,28 @@ void do_tests(struct pci_native_driver_interface *pci)
     printf("finished (took %f seconds).\r\n", time / 1000.0);
 }
 
+/*
+ * temporarily replace the trap 0 handler with this so we can avoid
+ * getting caught by BaS versions that don't understand the driver interface
+ * exposure call.
+ * If we get here, we have a BaS version that doesn't support the trap 0 interface
+ */
+static void __attribute__((interrupt)) trap0_catcher(void)
+{
+    __asm__ __volatile__(
+        "       clr.l       d0              \n\t"       // return 0 to indicate not supported
+        :
+        :
+        :
+    );
+}
+
 struct driver_table *get_bas_drivers(void)
 {
     struct driver_table *ret = NULL;
+    void *old_vector;
+
+    old_vector = Setexc(0x20, trap0_catcher);               /* set our own temporarily */
 
     __asm__ __volatile__(
         "           bra.s   do_trap             \n\t"
@@ -71,6 +90,7 @@ struct driver_table *get_bas_drivers(void)
         :                       /* no inputs */
         :                       /* clobbered */
     );
+    (void) Setexc(0x20, old_vector);                        /* restore original vector */
 
     return ret;
 }
@@ -83,7 +103,17 @@ void pci_test(void)
     struct pci_native_driver_interface *pci_driver = NULL;
 
     bas_drivers = get_bas_drivers();
-    printf("BaS version: %ld.%02ld\r\n", (long) bas_drivers->bas_version, (long) bas_drivers->bas_revision);
+    if (bas_drivers != NULL && bas_drivers != -1L)
+    {
+        printf("BaS driver vector: %p\r\n", bas_drivers);
+        printf("BaS version: %ld.%02ld\r\n", (long) bas_drivers->bas_version, (long) bas_drivers->bas_revision);
+    }
+    else
+    {
+        printf("BaS driver retrieval not supported\r\n");
+        printf("(old BaS version or FireTOS?)\r\n");
+        exit(1);
+    }
 
     ifc = bas_drivers->interfaces;