Subversion Repositories openrisc_me

/*
 *
 * Communicate with the verilog testbench via memory, send commands for it to
 * inject errors to I/D cache content RAM and tag RAM, I/D MMU translate and 
 * match RAMs, and the register file RAM.
 *
 * On instruction/data cache parity error we should receive an interrupt.
 *
 * On I/MMU parity error we should get an TLB miss exception, causing a software
 * flush before continuing.
 *
 * On register file parity error we reset via assertion of reset signal to 
 * OR1200.
 *
 * To allow some variables to maintain their values between resets, we store
 * them in #defined memory locations.
 * 
 * The interrupt and exception vector routines contain important code allowing
 * the test to proceed, so if they don't fire (due to parity errors not being
 * injected, exercised or detected properly) then the test will repeat itself
 * over and over with the same values, and should be stopped by the user and
 * investigated.
 *
 * The ft_state pointer points to the overall state of the test.
 *
 * Julius Baxter, julius@opencores.org
 *
 */
 
 
#include "cpu-utils.h"
#include "spr-defs.h"
#include "printf.h"
#include "board.h"
 
// define to jump to a certain test immediately
//#define JUMP_TO_TEST 3
 
#define FT_MEM 0x4
#define FT_GO_COMMAND 0x88000000
 
#define FT_STIM_CMD_OFF 24
 
#define FT_STIM_CMD_IC_RAM_FAULT 0x1
#define FT_STIM_CMD_IC_TAG_FAULT 0x2
#define FT_STIM_CMD_DC_RAM_FAULT 0x3
#define FT_STIM_CMD_DC_TAG_FAULT 0x4
 
#define FT_STIM_CMD_DMMU_MR_FAULT 0x5
#define FT_STIM_CMD_DMMU_TR_FAULT 0x6
#define FT_STIM_CMD_IMMU_MR_FAULT 0x7
#define FT_STIM_CMD_IMMU_TR_FAULT 0x8
#define FT_STIM_CMD_RF_FAULT 0x9
 
#define FT_STIM_CMD_DEBUG_ON (0xff<<FT_STIM_CMD_OFF)
#define FT_STIM_CMD_DEBUG_OFF (0xfe<<FT_STIM_CMD_OFF)
 
 
#define FT_STIM_WRITE(x) REG32(FT_MEM)=x
#define FT_STIM_WRITE_FAULT_CMD(x,y,z) \
  REG32(FT_MEM)=(x<<FT_STIM_CMD_OFF)|(((y)&0xff)<<16)|((z)&0xffff)
 
#define FT_STIM_WAIT_ACK while(REG32(FT_MEM)!=0)
 
#define FT_STATE_RECORD_MEM 0x8
#define FT_STATE_TEST_STATE1_MEM 0xc
#define FT_STATE_TEST_STATE2_MEM 0x10
#define FT_STATE_TEST_STATE3_MEM 0x14
 
#define PAGE_SIZE 8192
 
extern unsigned long _stack;
 
void
enable_bench_debug(void)
{
	FT_STIM_WRITE(FT_STIM_CMD_DEBUG_ON);
	FT_STIM_WAIT_ACK;
}
 
void
disable_bench_debug(void)
{
	FT_STIM_WRITE(FT_STIM_CMD_DEBUG_OFF);
	FT_STIM_WAIT_ACK;
}
 
 
void
dmmu_tlb_one_to_one_reset(void)
{
 
	int i, j, sets, ways;
	unsigned long dmmucfgr;
 
	// Determine number of TLB sets
	dmmucfgr = mfspr(SPR_DMMUCFGR);
 
	ways = (dmmucfgr & SPR_DMMUCFGR_NTW) + 1;
	sets = 1 << ((dmmucfgr & SPR_DMMUCFGR_NTS)>> SPR_DMMUCFGR_NTS_OFF);
 
 
 
	// Initialise all match/translate register pairs, set 1-1 mapping
	for (i = 0; i < ways; i++)
		for (j = 0; j < sets; j++){
			mtspr (SPR_DTLBMR_BASE(i) + j, (j*PAGE_SIZE) | 
			       SPR_DTLBMR_V);
			mtspr (SPR_DTLBTR_BASE(i) + j, (j*PAGE_SIZE) | 
			       DTLB_PR_NOLIMIT);
		}
 
	// Set cache inhibit for first page
	mtspr (SPR_DTLBTR_BASE(0), 0 | DTLB_PR_NOLIMIT | SPR_DTLBTR_CI);
 
}
 
 
// If DC is present, init DMMU to disable cache on first page, which we'll
// use to communicate to the fault injection testbench.
int 
dmmu_init(void)
{
 
	unsigned long upr;
 
	// Only do this if DMMU and DC are here
 
	// Get the unit present register
	upr = mfspr(SPR_UPR);
	if ((upr & SPR_UPR_DCP) && !(upr & SPR_UPR_DMP))
		// Cannot do the test
		exit(1);
 
	if (!(upr & SPR_UPR_DCP))
		// No need to config MMU - no data cache
		return 1;
 
	dmmu_tlb_one_to_one_reset();
 
	/* Enable DMMU */
	lo_dmmu_en ();
 
	return 0;
 
}
 
// Initialise iMMU
void
immu_tlb_one_to_one_reset(void)
{
 
	int i, j, sets, ways;
	unsigned long immucfgr;
 
	// Determine number of TLB sets
	immucfgr = mfspr(SPR_IMMUCFGR);
 
	ways = (immucfgr & SPR_IMMUCFGR_NTW) + 1;
	sets = 1 << ((immucfgr & SPR_IMMUCFGR_NTS)>> SPR_IMMUCFGR_NTS_OFF);
 
	// Initialise all match/translate register pairs, set 1-1 mapping
	for (i = 0; i < ways; i++)
		for (j = 0; j < sets; j++){
			mtspr (SPR_ITLBMR_BASE(i) + j, (j*PAGE_SIZE) | 
			       SPR_ITLBMR_V);
			mtspr (SPR_ITLBTR_BASE(i) + j, (j*PAGE_SIZE) | 
			       ITLB_PR_NOLIMIT);
		}
}
 
void
test_state_update(void)
{
	volatile int * test_state1 = (volatile int*)FT_STATE_TEST_STATE1_MEM;
	volatile int * test_state2 = (volatile int*)FT_STATE_TEST_STATE2_MEM;
	volatile int * test_state3 = (volatile int*)FT_STATE_TEST_STATE3_MEM;
	volatile int * ft_state = (volatile int*)FT_STATE_RECORD_MEM;
 
	// Disable DC here
	mtspr(SPR_SR, (mfspr(SPR_SR) & ~SPR_SR_DCE));
	// Flush line of test state
	mtspr(SPR_DCBIR, (unsigned long) test_state1);
 
	(*test_state1)++;
 
	if (*test_state1 == *test_state2)
		(*ft_state)++;
 
}
 
// Initialise TLB contents and enable iMMU
int
immu_init(void)
{
 
	int i, j, sets, ways;
	unsigned long upr;
	unsigned long immucfgr;
 
	// Only do this if iMMU here
 
	// Get the unit present register
	upr = mfspr(SPR_UPR);
	if (!(upr & SPR_UPR_IMP))
		return 1;
 
	immu_tlb_one_to_one_reset();
 
	/* Enable IMMU */
	lo_immu_en ();
 
	return 0;
}
 
void
itlb_miss_handler (void)
{
	// This was probably cause by injection of parity error
	// We just reset iTLB in event of miss
	printf("iTLB miss handler resetting iTLB\n");
 
	immu_tlb_one_to_one_reset();
 
	test_state_update();
 
}
 
void
dtlb_miss_handler (void)
{
	// This was probably cause by injection of parity error
	// We just reset iTLB in event of miss
	printf("dTLB miss handler resetting iTLB\n");
 
	dmmu_tlb_one_to_one_reset();
 
	test_state_update();
 
}
 
 
unsigned long 
inline inline_mfspr(unsigned long spr)
{	
  unsigned long value;
  asm("l.mfspr\t\t%0,%1,0" : "=r" (value) : "r" (spr));
  return value;
}
 
void
parity_error_interrupt(void)
{
	// Read parity error indicator. Reading also clears interrupt line.
	char parerr = REG8(PARERR_BASE);
 
	printf("Parity error interrupt: 0x%02x\n",parerr);
 
	test_state_update();	
 
	// Clear iTLB
	//immu_tlb_one_to_one_reset();
 
	return;
}
 
 
void
dummy_function()
{
	volatile int i;
 
	report(0xdfdfdfdf);
 
	for(i=0;i<64;i++);
 
	return;
}
 
void
copy_function(void *insn_area, void* dummy_function, int nwords)
{
	int i;
	unsigned long *p1 = (unsigned long *)insn_area;
	unsigned long *p2 = (unsigned long *)dummy_function;
	for (i = 0; i < nwords; i++)
		p1[i] = p2[i];
}
 
 
void
ic_test_setup()
{
	//Initialise handler vector
	int_init();
	int_add(PARERR_IRQ, parity_error_interrupt, 0);
 
	// Enable interrupts in supervisor register
	cpu_enable_user_interrupts();
}
 
 
void
dc_test_setup()
{
	//Initialise handler vector
	int_init();
	int_add(PARERR_IRQ, parity_error_interrupt, 0);
 
	// Enable interrupts in supervisor register
	cpu_enable_user_interrupts();
}
 
 
int
dmmu_test_setup(unsigned long data_location)
{
	unsigned long mmucfgr;
	int mmu_tlb_reg, mmusets;
 
	// Determine number of mmusets
	mmucfgr = mfspr (SPR_DMMUCFGR);
	mmusets = 1 << ((mmucfgr & SPR_DMMUCFGR_NTS)>> 
			SPR_DMMUCFGR_NTS_OFF);
	// Now get TLB register that will be used when matching/translating
	// VA for page dummy_function() is in.
 
	mmu_tlb_reg = (data_location / PAGE_SIZE) % mmusets;
	report (data_location);
	report (mmu_tlb_reg);
 
	// Enable interrupts and install handler. Error will trigger
	// an interrupt.
 
	//Initialise handler vector
	int_init();
	int_add(PARERR_IRQ, parity_error_interrupt, 0);
 
	// Add handler for dTLB miss (parity error may show up as
	// miss.)
	add_handler(0x9, dtlb_miss_handler);
 
	// Enable interrupts in supervisor register
	cpu_enable_user_interrupts();
 
	return mmu_tlb_reg;
 
}
 
 
 
int
immu_test_setup(void)
{
	unsigned long mmucfgr;
	int mmu_tlb_reg, mmusets;
 
	// Determine number of mmusets
	mmucfgr = mfspr(SPR_IMMUCFGR);
	mmusets = 1 << ((mmucfgr & SPR_IMMUCFGR_NTS)>> 
			SPR_IMMUCFGR_NTS_OFF);
	// Now get TLB register that will be used when matching/translating
	// VA for page dummy_function() is in.
 
	mmu_tlb_reg = ((unsigned long)dummy_function / PAGE_SIZE) % mmusets;
 
	report(mmu_tlb_reg);
 
	// Enable interrupts and install handler. Error will trigger
	// an interrupt.
 
	// iMMU parity errors trigger both a miss and the parity interrupt, but
	// for now we'll just have the miss vector do the handling
 
	//Initialise handler vector
	//int_init();
	//int_add(PARERR_IRQ, parity_error_interrupt, 0);
	// Enable interrupts in supervisor register
	//cpu_enable_user_interrupts();
 
	// Add handler for iTLB miss (parity error may show up as
	// miss.)
	add_handler(0xa, itlb_miss_handler);
 
 
	return mmu_tlb_reg;	
}
 
 
void
gpr_test(int reg, int bit_number)
{
 
	switch(reg)
	{
	case 0:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r0");
 
		break;
 
	case 1:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r1");
 
		break;
 
	case 2:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r2");
 
		break;
 
	case 3:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r3");
 
		break;
 
	case 4:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r4");
 
		break;
 
	case 5:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r5");
 
		break;
 
	case 6:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r6");
 
		break;
 
	case 7:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r7");
 
		break;
 
	case 8:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r8");
 
		break;
 
	case 9:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r9");
 
		break;
 
	case 10:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r10");
 
		break;
 
	case 11:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r11");
 
		break;
 
	case 12:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r12");
 
		break;
 
	case 13:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r13");
 
		break;
 
	case 14:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r14");
 
		break;
 
	case 15:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r15");
 
		break;
 
	case 16:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r16");
 
		break;
 
	case 17:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r17");
 
		break;
 
	case 18:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r18");
 
		break;
 
	case 19:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r19");
 
		break;
 
	case 20:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r20");
 
		break;
 
	case 21:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r21");
 
		break;
 
	case 22:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r22");
 
		break;
 
	case 23:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r23");
 
		break;
 
	case 24:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r24");
 
		break;
 
	case 25:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r25");
 
		break;
 
	case 26:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r26");
 
		break;
 
	case 27:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r27");
 
		break;
 
	case 28:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r28");
 
		break;
 
	case 29:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r29");
 
		break;
 
	case 30:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r30");
 
		break;
 
	case 31:
		FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_RF_FAULT,bit_number, reg);
		FT_STIM_WAIT_ACK;
		__asm__ ("l.add r23, r24, r31");
		break;
	}
}
 
 
int main(void)
{
 
	unsigned long cachecfgr, mmucfgr;
	int cachebs, cachesets, cachesize;
	int mmusets;
	unsigned long * stack_ptr;
	unsigned long * data_area;
	unsigned long * insn_area;
	int mmu_mr, mmu_tr;
	int bit_number, word_number;
 
	printf("\nParity Error Test Restart\n");
 
	// Setup DMMU with cache disable on first page
	dmmu_init();
 
	// This ft_state variable should retain its value between resets, as
	// we'll always store it back to its memory location when accessing it.
	volatile int * ft_state = (volatile int*)FT_STATE_RECORD_MEM;
 
	volatile int * test_state1 = (volatile int*)FT_STATE_TEST_STATE1_MEM;
	volatile int * test_state2 = (volatile int*)FT_STATE_TEST_STATE2_MEM;
 
 
	printf("Test state %d\n",*ft_state);
 
restart_tests:
 
	switch(*ft_state)
	{
	case 0:
		printf("State %d: Test initialisation\n", *ft_state);
 
		// Kickoff - tell testbench stimulus to start
		printf("Writing GO command\n");
		FT_STIM_WRITE(FT_GO_COMMAND);
		(*ft_state)++;
		FT_STIM_WAIT_ACK;
 
#ifdef JUMP_TO_TEST		
		*ft_state = JUMP_TO_TEST;
		goto restart_tests;
#endif
 
		// No break, fall through to case 1
	case 1:
		printf("\nState %d: IC instruction memory test\n", *ft_state);
 
		// IC instruction RAM fault injection.
 
		ic_test_setup();
 
		// Determine cache configuration
		cachecfgr = inline_mfspr(SPR_ICCFGR);
		// What is block size
		cachebs = (cachecfgr & SPR_ICCFGR_CBS) ? 32 : 16;
 
		cachesets = (1 << ((cachecfgr & SPR_ICCFGR_NCS)>> 
				   SPR_ICCFGR_NCS_OFF));
 
		// *ft_state increments when *test_state1 == *test_state2
		*test_state1 = 0;
		*test_state2 = 32;
 
		word_number = (((unsigned long)dummy_function >> 2) % 
			       cachesets);
		report((unsigned long) dummy_function);
		report(word_number);
 
		// Call a function, so it's cached, inject an error into
		// the cache instruction RAM location where the first
		// instruction of that function is located, and call it again.
		// This should trip the parity error, and the interrupt.
 
		while(*ft_state == 1)
		{
			dummy_function();
 
			bit_number = *test_state1;
			FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_IC_RAM_FAULT,
						bit_number, word_number);
 
			FT_STIM_WAIT_ACK;
 
			dummy_function();
 
		}
	case 2:
		printf("\nState %d: IC tag memory test\n", *ft_state);
		// IC tag RAM fault injection.
 
		ic_test_setup();
 
		// Determine cache configuration
		cachecfgr = inline_mfspr(SPR_ICCFGR);
		// What is block size
		cachebs = (cachecfgr & SPR_ICCFGR_CBS) ? 32 : 16;
 
		cachesets = (1 << ((cachecfgr & SPR_ICCFGR_NCS)>> 
				   SPR_ICCFGR_NCS_OFF));
 
		cachesize = cachebs * cachesets;
 
		// Same as above, but this time for the tag RAM.
 
		// *ft_state increments when *test_state1 == *test_state2
		*test_state1 = 0;
		*test_state2 = 24;
 
		// Calculate word in tag RAM
		word_number = (((unsigned long)dummy_function % 
				cachesize ) / cachebs) ;
 
		while(*ft_state == 2)
		{
			dummy_function();
			bit_number = *test_state1;
			FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_IC_TAG_FAULT,
						bit_number, word_number);
 
			FT_STIM_WAIT_ACK;
 
			dummy_function();
 
		}
	case 3:
		// DC data RAM fault injection
		printf("\nState %d: DC data memory test\n", *ft_state);
 
		dc_test_setup();
 
		// Read a far off data address (outside first MMU page, at 
		// least), determine it's cache line, inject a fault and read 
		// it again.
		// Set up stack pointer variable, so we know where top of
		// stack is.
		stack_ptr = (unsigned long*)&_stack;
 
		// Put test area one page past it:
		data_area = stack_ptr;
		data_area += PAGE_SIZE;
 
		// Report address
		report((unsigned long)data_area);
		// Write a test variable there
		*data_area = 0xea5ecafe;
 
		// Determine cache set up
		cachecfgr = inline_mfspr(SPR_DCCFGR);
		// What is block size
		cachebs = (cachecfgr & SPR_DCCFGR_CBS) ? 32 : 16;
 
		cachesets = (1 << ((cachecfgr & SPR_DCCFGR_NCS)>> 
				   SPR_DCCFGR_NCS_OFF));
 
		cachesize = cachebs * cachesets;
 
		// Calculate address of word in DC RAM
		word_number = (((unsigned long)data_area % cachesize) >> 2);
 
		// *ft_state increments when *test_state1 == *test_state2
		*test_state1 = 0;
		*test_state2 = 32;
 
		while(*ft_state == 3)
		{
 
			// Read the location, to cache it, or trigger read
			// from cache
			report(*data_area);
			bit_number = *test_state1;
			FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_DC_RAM_FAULT,
						bit_number, word_number);
 
			FT_STIM_WAIT_ACK;
 
			//report(*data_area);
 
 
		}
	case 4:
		printf("\nState %d: DC tag memory test\n", *ft_state);
 
		dc_test_setup();
 
		// Same set up as data cache RAM test.
		stack_ptr = (unsigned long*)&_stack;
 
		// Put test area one page past it:
		data_area = stack_ptr;
		data_area += PAGE_SIZE;
 
		// Report address
		report((unsigned long)data_area);
		// Write a test variable there
		*data_area = 0xea5ecafe;
 
		// Determine cache set up
		cachecfgr = inline_mfspr(SPR_DCCFGR);
		// What is block size
		cachebs = (cachecfgr & SPR_DCCFGR_CBS) ? 32 : 16;
 
		cachesets = (1 << ((cachecfgr & SPR_DCCFGR_NCS)>> 
				   SPR_DCCFGR_NCS_OFF));
 
		cachesize = cachebs * cachesets;
 
		// Calculate address of word in DC RAM
		word_number = (((unsigned long)data_area % cachesize)/cachebs);
 
		// *ft_state increments when *test_state1 == *test_state2
		*test_state1 = 0;
		*test_state2 = 24;
 
		while(*ft_state == 4)
		{
 
			// Read the location, to cache it, or trigger read
			// from cache
			report(*data_area);
			bit_number = *test_state1;
			FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_DC_TAG_FAULT,
						bit_number, word_number);
			FT_STIM_WAIT_ACK;
 
			report(*data_area);
 
		}
	case 5:
		// Instruction MMU TLB match register corruption test
		// Parity error should just trigger an interrupt, not
		// reset the system
		printf("\nState %d: IMMU match register memory test\n", 
		       *ft_state);
 
		if  (immu_init())
		{
 
			printf("\nSkipping test %d - no iMMU present\n",
			       *ft_state);
			(*ft_state)++;
			goto restart_tests;
		}
 
		mmu_mr = immu_test_setup();
 
		*test_state1 = 0;
		*test_state2 = 16;
 
		while (*ft_state==5)
		{
			bit_number = *test_state1;
			FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_IMMU_MR_FAULT,
						bit_number, mmu_mr);
			FT_STIM_WAIT_ACK;
 
			dummy_function();
		}
 
 
 
	case 6:
		// Instruction MMU TLB match register corruption test
		// Should just trigger interrupt, not reset.
		printf("\nState %d: IMMU translate register memory test\n", 
		       *ft_state);
		if  (immu_init())
		{
 
			printf("\nSkipping test %d - no iMMU present\n",
			       *ft_state);
			(*ft_state)++;
			goto restart_tests;
		}
 
		mmu_tr = immu_test_setup();
 
		*test_state1 = 0;
		*test_state2 = 24;
 
		while (*ft_state==6)
		{
			bit_number = *test_state1;
			FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_IMMU_TR_FAULT,
						bit_number, mmu_mr );
			FT_STIM_WAIT_ACK;
 
			dummy_function();
		}
 
	case 7:
		// Data MMU TLB match register corruption test
		// Parity error should just trigger an interrupt, not
		// reset the system
		printf("\nState %d: DMMU match register memory test\n", 
		       *ft_state);
 
		if  (dmmu_init())
		{
 
			printf("\nSkipping test %d - no dMMU present\n",
			       *ft_state);
			(*ft_state)++;
			goto restart_tests;
		}
 
		// Choose address for test
		data_area = stack_ptr;
		data_area += PAGE_SIZE;
 
		*data_area = 0xeaaaa51e;
 
		mmu_mr = dmmu_test_setup((unsigned long) data_area);
 
		*test_state1 = 0;
		*test_state2 = 16;
 
		while (*ft_state==7)
		{
		        bit_number = *test_state1;
			FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_DMMU_MR_FAULT,
						bit_number, mmu_mr );
			FT_STIM_WAIT_ACK;
 
			report (*data_area);
		}
 
	case 8:
		// Data MMU TLB translate register corruption test
		// Parity error should just trigger an interrupt, not
		// reset the system
		printf("\nState %d: DMMU translate register memory test\n", 
		       *ft_state);
 
		if  (dmmu_init())
		{
 
			printf("\nSkipping test %d - no dMMU present\n",
			       *ft_state);
			(*ft_state)++;
			goto restart_tests;
		}
 
		// Choose address for test
		data_area = stack_ptr;
		data_area += (PAGE_SIZE<<2);
 
		*data_area = 0xdeadc0de;
 
		mmu_tr = dmmu_test_setup((unsigned long) data_area);
 
		*test_state1 = 0;
		*test_state2 = 24;
 
		while (*ft_state==8)
		{
			bit_number = *test_state1;
			FT_STIM_WRITE_FAULT_CMD(FT_STIM_CMD_DMMU_TR_FAULT,
						bit_number, mmu_tr );
			FT_STIM_WAIT_ACK;
 
			report (*data_area);
		}
 
		// Sometimes the interrupt triggers twice here, meaning
		// ft_state was incremented twice. So manually put it back,
		// just in case.
		*ft_state = 9;
 
	case 9:
		printf("\nState %d: Register file memory test\n", 
		       *ft_state);
 
		// Inject an error into a reg, then schedule an instruction to
		// read it.
		(*ft_state)++;
 
		// initialise variables here
		*test_state1 = -1;  // Bit number
		*test_state2 = 0; // GPR number
 
	case 10:
		// Sit in this state to do the tests
		(*test_state1)++;
 
		if (*test_state1 == 32)
		{
			(*test_state2)++;
			*test_state1 = 0;
 
			if (*test_state2 == 32)
				(*ft_state)++;
		}
 
		bit_number = *test_state1;
 
		while (*ft_state==10)
		{
			gpr_test(*test_state2,bit_number);
		}
 
	case 11:
		printf("Tests done\n");
		report(0x8000000d);
		return 0;
	default:
		// Shouldn't be here. ft_state somhow got corrupted.
		exit(1);
		break;
	}
 
	return 0;
}