diff --git a/sources/cache.c b/sources/cache.c new file mode 100644 index 0000000..3397d6f --- /dev/null +++ b/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" + ); +} diff --git a/sources/cache.h b/sources/cache.h new file mode 100644 index 0000000..8e306d6 --- /dev/null +++ b/sources/cache.h @@ -0,0 +1,9 @@ +#ifndef _CACHE_H_ +#define _CACHE_H_ + +#include + +extern void flushDataCacheRegion(void *adr, uint32_t length); +extern void flushInstructionCacheRegion(void *adr, uint32_t length); + +#endif /* _CACHE_H_ */ diff --git a/sources/sysinit.c b/sources/sysinit.c index 3e5b670..34dec83 100644 --- a/sources/sysinit.c +++ b/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" - "lea _BaS,A1\n\t" - "sub.l A0,A1\n\t" - "move.l #_Bas_base,A2\n\t" - "move.l A2,A3\n\t" - "add.l A1,A3\n\t" - "lea copy_end,A4\n\t" -"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" - : :); + " .global _copy_start | \n\t" + " move.l %0,a3 | calculated start address\n\t" + " jmp (a3) | go! \n\t" + "_copy_start: | \n\t" + /* output */ : + /* input */ : "g" (jmp) + /* clobber */: "a3" + ); }