From 628ba49c83acf035cc1fe21575699e2b79c4d5c9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Markus=20Fr=C3=B6schle?= <mfro@mubf.de>
Date: Mon, 29 Sep 2014 22:26:25 +0000
Subject: [PATCH] fixed driver_vec retrieval. Sholdn't crash anymore on
 FireTOS.

---
 BaS_gcc/BaS_gcc.files                 |  1 +
 BaS_gcc/BaS_gcc.includes              |  1 +
 BaS_gcc/if/driver_vec.c               | 30 ++++++++++++++++++++++-----
 BaS_gcc/tos/Makefile                  |  6 +++---
 BaS_gcc/tos/bascook/sources/bascook.c | 29 +++++++++++++++++++++++---
 5 files changed, 56 insertions(+), 11 deletions(-)

diff --git a/BaS_gcc/BaS_gcc.files b/BaS_gcc/BaS_gcc.files
index 06d653b..e83bdf1 100644
--- a/BaS_gcc/BaS_gcc.files
+++ b/BaS_gcc/BaS_gcc.files
@@ -269,3 +269,4 @@ tos/bascook/sources/bascook.c
 tos/Makefile
 usb/usb_hub.c
 include/usb_hub.h
+tos/Makefile
diff --git a/BaS_gcc/BaS_gcc.includes b/BaS_gcc/BaS_gcc.includes
index 8b4dcf6..1f77a67 100644
--- a/BaS_gcc/BaS_gcc.includes
+++ b/BaS_gcc/BaS_gcc.includes
@@ -1,2 +1,3 @@
 include
 /usr/m68k-elf/include
+tos
diff --git a/BaS_gcc/if/driver_vec.c b/BaS_gcc/if/driver_vec.c
index 8c38fd2..96f3d83 100644
--- a/BaS_gcc/if/driver_vec.c
+++ b/BaS_gcc/if/driver_vec.c
@@ -137,9 +137,9 @@ static struct framebuffer_driver_interface framebuffer_interface =
  */
 static struct mmu_driver_interface mmu_interface =
 {
-    .map_page_locked = &mmu_map_data_page_locked,
-    .unlock_page = &mmu_unlock_data_page,
-    .report_locked_pages = &mmu_report_locked_pages
+	.map_page_locked = &mmu_map_data_page_locked,
+	.unlock_page = &mmu_unlock_data_page,
+	.report_locked_pages = &mmu_report_locked_pages
 };
 
 static struct generic_interface interfaces[] =
@@ -206,8 +206,28 @@ static struct driver_table bas_drivers =
 
 void __attribute__((interrupt)) get_bas_drivers(void)
 {
-	__asm__ __volatile__(
-		"move.l	#%[drivers],d0\n\t"
+	__asm__ __volatile(
+		/*
+		 * sp should now point to the next instruction after the trap
+		 * The trap itself is 2 bytes, the four bytes before that must
+		 * read '_BAS' or we are not meant by this call
+		 */
+		"		move.l	d0,-(sp)				\n\t"	// save register
+		"		move.l	10(sp),d0				\n\t"	// get "magic word"
+		"		cmp.l	#0x5f424153,d0			\n\t"	// is it '_BAS'?
+		"		beq		fetch_drivers			\n\t"	// yes
+		/*
+		 * This seems indeed a "normal" trap #0. Better pass control to "normal" trap #0 processing
+		 * If trap #0 isn't set to something sensible, we'll probably crash here, but this must be
+		 * prevented on the caller side.
+		 */
+		"		move.l	(sp)+,d0				\n\t"	// restore register
+		"		move.l	0x80,-(sp)				\n\t"	// fetch vector
+		"		rts								\n\t"	// and jump through it
+
+		"fetch_drivers:							\n\t"
+		"		move.l	#%[drivers],d0			\n\t"	// return driver struct in d0
+		"		addq.l	#4,sp					\n\t"	// adjust stack
 		:	                            /* no output */
 		:	[drivers] "o" (bas_drivers)		/* input */
 		:									/* clobber */
diff --git a/BaS_gcc/tos/Makefile b/BaS_gcc/tos/Makefile
index 4fde9fe..ff1ff30 100644
--- a/BaS_gcc/tos/Makefile
+++ b/BaS_gcc/tos/Makefile
@@ -1,11 +1,11 @@
 .PHONY: tos
 .PHONY: jtagwait
-.PHONY: mcdcook
-tos: jtagwait mcdcook
+.PHONY: bascook
+tos: jtagwait bascook
 
 jtagwait:
 	(cd $@; make)
 
 bascook:
 	(cd $@; make)
-	
+
diff --git a/BaS_gcc/tos/bascook/sources/bascook.c b/BaS_gcc/tos/bascook/sources/bascook.c
index 877a13d..2ae050c 100644
--- a/BaS_gcc/tos/bascook/sources/bascook.c
+++ b/BaS_gcc/tos/bascook/sources/bascook.c
@@ -10,8 +10,10 @@ struct driver_table *get_bas_drivers(void)
 	struct driver_table *ret = NULL;
 
 	__asm__ __volatile__(
-		"	trap	#0\n\t"
-		"	move.l	d0,%[ret]\n\t"
+		"			bra.s	 do_trap			\n\t"
+		"			.dc.l	0x5f424153			\n\t"	// '_BAS'
+		"do_trap:	trap	#0					\n\t"
+		"			move.l	d0,%[ret]			\n\t"
 		:	[ret] "=m" (ret)	/* output */
 		:						/* no inputs */
 		:						/* clobbered */
@@ -20,6 +22,22 @@ struct driver_table *get_bas_drivers(void)
 	return ret;
 }
 
+/*
+ * 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)) my_own_trap0_handler(void)
+{
+	__asm__ __volatile__(
+		"		clr.l		d0				\n\t"		// return 0 to indicate not supported
+		:
+		:
+		:
+	);
+}
+
 static uint32_t cookieptr(void)
 {
 	return * (uint32_t *) 0x5a0L;
@@ -86,11 +104,16 @@ int main(int argc, char *argv[])
 {
 	struct driver_table *dt;
 	void *ssp;
+	void *old_vector;
 
 	(void) Cconws("retrieve BaS driver interface\r\n");
 
 	ssp = (void *) Super(0L);
-	dt = get_bas_drivers();
+
+	old_vector = Setexc(0x20, my_own_trap0_handler);		/* set our own temporarily */
+	dt = get_bas_drivers();									/* trap #1 */
+	(void) Setexc(0x20, old_vector);						/* restore original vector */
+
 	if (dt)
 	{
 		struct generic_interface *ifc = &dt->interfaces[0];