Translated BaS copy routines to C.

Added functionality to flush caches before jumping into copied code

sources/cache.c

@@ -0,0 +1,61 @@
+/*
+ * cache handling
+ */
+#include "cache.h"
+
+
+void flushDataCacheRegion(void *adr, uint32_t length)
+{
+	asm volatile
+	(
+		"		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_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 flushInstructionCacheRegion(void *adr, uint32_t length)
+{
+	asm volatile
+	(
+		"		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"
+	);
+}

sources/cache.h

@@ -0,0 +1,9 @@
+#ifndef _CACHE_H_
+#define _CACHE_H_
+
+#include <stdint.h>
+
+extern void flushDataCacheRegion(void *adr, uint32_t length);
+extern void flushInstructionCacheRegion(void *adr, uint32_t length);
+
+#endif /* _CACHE_H_ */

sources/sysinit.c

@@ -767,6 +767,12 @@ ac97_end:
 }
 
 void initialize_hardware(void) {
+	extern uint8_t *copy_start;
+	extern uint8_t *Bas_base;
+	uint32_t *src;
+	uint32_t *dst;
+	uint32_t jmp;
+
 asm(
 	"move.l 	#0x000C8120,D0\n\t"
 	"move.l		D0,_rt_cacr\n\t"
@@ -795,25 +801,27 @@ asm(
 		init_ac97();
 	}
 
+	/* copy the BaS code contained in flash just behind us */
+	src = copy_start;
+	dst = Bas_base;
+	jmp = (uint8_t *) BaS - copy_start + Bas_base;
+	do {
+		*src++ = *dst++;
+		*src++ = *dst++;
+		*src++ = *dst++;
+		*src++ = *dst++;
+	} while (src < (uint8_t *) copy_end);
+
+	flushDataCacheRegion(Bas_base, (uint8_t *) copy_end - copy_start);
+	flushInstructionCacheRegion(Bas_base, (uint8_t *) copy_end - copy_start);
+
 	asm volatile(
-		"lea		copy_start,A0\n\t"
+		"		.global	_copy_start	| \n\t"
-		"lea		_BaS,A1\n\t"
+		"		move.l	%0,a3		| calculated start address\n\t"
-		"sub.l		A0,A1\n\t"
+		"		jmp		(a3)		| go! \n\t"
-		"move.l		#_Bas_base,A2\n\t"
+		"_copy_start:				| \n\t"
-		"move.l		A2,A3\n\t"
+		/* output */ :
-		"add.l		A1,A3\n\t"
+		/* input */  : "g" (jmp)
-		"lea		copy_end,A4\n\t"
+		/* clobber */: "a3"
-"BaS_copy_loop:		/* copy 16 bytes per turn */\n\t"
+		);
-		"move.l		(A0)+,(A2)+\n\t"
-		"move.l		(A0)+,(A2)+\n\t"
-		"move.l		(A0)+,(A2)+\n\t"
-		"move.l		(A0)+,(A2)+\n\t"
-		"cmp.l		A4,A0\n\t"
-		"blt		BaS_copy_loop\n\t"
-"\n\t"
-		"intouch	A3\n\t"	/* FIXME: we'd better update caches to contain the data we just copied */
-		"jmp		(A3)\n\t"
-"copy_start:\n\t"
-		"nop\n\t"
-		 : :);
 }