diff --git a/sources/BaS.c b/sources/BaS.c
index ec62973..158dd14 100644
--- a/sources/BaS.c
+++ b/sources/BaS.c
@@ -7,6 +7,7 @@
 #include "MCF5475.h"
 #include "MCF5475_SLT.h"
 #include "startcf.h"
+#include "cache.h"
 
 extern uint32_t Bas_base[];
 extern uint8_t tos_base[];
@@ -77,6 +78,9 @@ void BaS(void)
 		}
 	}
 
+	/* we have copied a code area, so flush the caches */
+	flush_and_invalidate_caches();
+
 #ifdef _NOT_USED_
 	/*
 	 * set the NVRAM checksum as invalid
diff --git a/sources/cache.c b/sources/cache.c
index 1cb9f9b..940b7e5 100644
--- a/sources/cache.c
+++ b/sources/cache.c
@@ -3,70 +3,9 @@
  */
 #include "cache.h"
 
-
-void flushDataCacheRegion(void *adr, uint32_t length)
-{
-	__asm__
-	(
-		"		move.l		%0,d0		| start address\n\t"
-		"		move.l		d0,a1\n\t"
-		"		add.l		%1,a1		| add length\n\t"
-		"		clr.l		d1			| way counter\n\t"
-		"		and.l		#0xfffffff0,d0	| align start address\n\t"
-		".flush_dregion_way_loop:\n\t"
-		"		move.l		d0,a0		| initialize a0\n\t"
-		"		add.l		d1,a0		| set way index\n\t"
-		".flush_dregion_loop:\n\t"
-		"		cpushl		DC,(a0)		| flush and invalidate the cache line\n\t"
-		"		add.l		#0x10,a0	| increment to next cache line\n\t"
-		"		cmp.l		a0,a1		| done with region?\n\t"
-		"		bgt			.flush_dregion_loop\n\t"
-		"		addq.l		#1,d1		| increment way counter\n\t"
-		"		addq.l		#1,a1		| update stop address to reflect new way value\n\t"
-		"		cmp.l		#4,d1		| cache way\n\t"
-		"		bne			.flush_dregion_way_loop\n\t"
-	/* output */ :
-	/* input */  : "g" (adr),
-				   "g" (length)
-	/* clobber */: "d0", "d1", "a0", "a1"
-	);
-}
-
-void flushInstructionCacheRegion(void *adr, uint32_t length)
-{
-	__asm__
-	(
-		"		move.l		%0,d0		| start address\n\t"
-		"		move.l		d0,a1\n\t"
-		"		add.l		%1,a1		| add length\n\t"
-		"		clr.l		d1			| way counter\n\t"
-		"		and.l		#0xfffffff0,d0	| align start address\n\t"
-		".flush_iregion_way_loop:\n\t"
-		"		move.l		d0,a0		| initialize a0\n\t"
-		"		add.l		d1,a0		| set way index\n\t"
-		".flush_iregion_loop:\n\t"
-		"		cpushl		IC,(a0)		| flush and invalidate the cache line\n\t"
-		"		add.l		#0x10,a0	| increment to next cache line\n\t"
-		"		cmp.l		a0,a1		| done with region?\n\t"
-		"		bgt			.flush_iregion_loop\n\t"
-		"		addq.l		#1,d1		| increment way counter\n\t"
-		"		addq.l		#1,a1		| update stop address to reflect new way value\n\t"
-		"		cmp.l		#4,d1		| cache way\n\t"
-		"		bne			.flush_iregion_way_loop\n\t"
-	/* output */ :
-	/* input */  : "g" (adr),
-				   "g" (length)
-	/* clobber */: "d0", "d1", "a0", "a1"
-	);
-}
-
-void clear_caches(void)
+void flush_and_invalidate_caches(void)
 {
 	__asm__ (
-	"cpusha:\n\t"
-	"	move		sr,d2\n\t"
-	"	move.l		d2,-(sp)\n\t"
-	"	move		#0x2700,sr			| no interrupts\n\t"
 	"	clr.l		d0\n\t"
 	"	clr.l		d1\n\t"
 	"	move.l		d0,a0\n\t"
@@ -75,16 +14,14 @@ void clear_caches(void)
 	"	lea			0x10(a0),a0			| index+1\n\t"
 	"	addq.l		#1,d1				| index+1\n\t"
 	"	cmpi.w		#512,d1				| all sets?\n\t"
-	"	bne			cfa_setloop			| no->\n\t"
+	"	bne.s		cfa_setloop			| no->\n\t"
 	"	clr.l		d1\n\t"
 	"	addq.l		#1,d0\n\t"
 	"	move.l		d0,a0\n\t"
 	"	cmpi.w		#4,d0				| all ways?\n\t"
-	"	bne			cfa_setloop			| no->\n\t"
-	"	move.l		(sp)+,d2\n\t"
-	"	move.w		d2,sr				| restore previous interrupt mask\n\t"
+	"	bne.s		cfa_setloop			| no->\n\t"
 	/* input */	:
 	/* output */	:
-	/* clobber */	: "d0", "d1", "d2", "a0"
+	/* clobber */	: "d0", "d1", "a0"
 	);
 }
diff --git a/sources/cache.h b/sources/cache.h
index 8e306d6..a8d788d 100644
--- a/sources/cache.h
+++ b/sources/cache.h
@@ -3,7 +3,6 @@
 
 #include <stdint.h>
 
-extern void flushDataCacheRegion(void *adr, uint32_t length);
-extern void flushInstructionCacheRegion(void *adr, uint32_t length);
+extern void flush_and_invalidate_caches(void);
 
 #endif /* _CACHE_H_ */
diff --git a/sources/supervisor.S b/sources/supervisor.S
index 9afa84a..7988122 100644
--- a/sources/supervisor.S
+++ b/sources/supervisor.S
@@ -9,6 +9,7 @@
 .extern _rt_ssp;
 .extern _rt_usp;
 .extern ___MMUBAR
+.extern _flush_and_invalidate_caches
 
 /* Register read/write macros */
 #define MCF_MMU_MMUCR	__MMUBAR
@@ -554,29 +555,10 @@ pv_f_d16pc:
 //*****************************************************
 cpusha:	 
  	lea			-16(a7),a7
-	movem.l		d0-d2/a0,(a7)		// register sichern
-	move		sr,d2
-	nop
-	move		#0x2700,sr			// no interrupts
-
-	clr.l		d0
-	clr.l		d1
-	move.l		d0,a0
-cfa_setloop:
-	cpushl		bc,(a0)				// flush
-	lea			0x10(a0),a0			// index+1
-	addq.l		#1,d1				// index+1
-	cmpi.w		#512,d1				// alle sets?
-	bne			cfa_setloop			// nein->
-	clr.l		d1
-	addq.l		#1,d0
-	move.l		d0,a0
-	cmpi.w		#4,d0				// all ways?
-	bne			cfa_setloop			// nein->
-	move.w		d2,sr				// alte interrupt maske
-	movem.l		(a7),d0-d2/a0		// register zur�ck
-	lea			16(a7),a7
-
+	movem.l		d0-d1/a0-a1,(a7)	// backup C trash registers
+	jsr			_flush_and_invalidate_caches
+	movem.l		(a7),d0-d1/a0-a1	// restore C trash registers
+ 	lea			16(a7),a7
 	rts	
 //*******************************************************33
 
diff --git a/sources/sysinit.c b/sources/sysinit.c
index fb01cb8..54665ad 100644
--- a/sources/sysinit.c
+++ b/sources/sysinit.c
@@ -709,9 +709,8 @@ void initialize_hardware(void) {
 		*dst++ = *src++;
 	} while (dst < &bas_end);
 
-	/* clear all addresses touched during copy from all cache lines */
-	flushDataCacheRegion(&BaS, (uint32_t) (&bas_end - &BaS));
-	flushInstructionCacheRegion(&BaS, (uint32_t) (&bas_end - &BaS));
+	/* we have copied a code area, so flush the caches */
+	flush_and_invalidate_caches();
 
 	__asm__ __volatile__(
 		"		move.l	%0,a3		| calculated start address\n\t"