#ifdef BUP_DIAGS
#include <stdio.h>
#include <vxWorks.h>
#include <ioLib.h>
#include <tickLib.h>
#include "ipos.h"
#include "dapi.h"
#include "apilocal.h"
#include "host_ver.h"

int printDiags = 1;
// assume user mem = 8meg
int userMem = 0x800000;
int userMem1 = 0x800000;
int userMem2 = 0x800000;
int bup_bme = 1;

#define HIC_STRESS		9
#define BME_STRESS		10

//-----------------------------------------------------
UINT32 tprr (int junk, UINT32 addr) {
  UINT32 temp;

  temp = pio_read_return (addr);
  return temp;
}
//-----------------------------------------------------
void rmw_reg (UINT32 addr, UINT32 data, UINT32 lsw_valid)
{
  UINT32 temp0, temp1;

  pio_read (addr, (&temp0));
  if (lsw_valid) {
    temp0 = temp0 & 0xffff0000;
    temp1 = data & 0xffff;
  }
  else {
    temp0 = temp0 & 0xffff;
    temp1 = data & 0xffff0000;
  }
  temp0 = temp0 | temp1;
  pio_write( addr, temp0);
}
//-----------------------------------------------------
void poll_reg (UINT32 addr, UINT32 mask, UINT32 poll_value, int equal_to)
{
  UINT32 temp1, temp0;
  
  temp1 = addr & 0xfffffffc;
  pio_read (temp1, (&temp0));
  if (equal_to) {
    while((temp0 & mask) == poll_value )
    pio_read (temp1, (&temp0));
  }
  else {
    while((temp0 & mask) != poll_value )
    pio_read (temp1, (&temp0));
  }
}
//-----------------------------------------------------
void load_fmc_icache_mbup ()
{
  int i, cache_low, cache_high, cache;
  FILE *fp;

  if ((fp = fopen("/tgtsvr/frm_cpu_icache", "r")) == NULL)
    printf("Error opening file\n");
  for (i=0; i<200; i=i+2)
    {
      fscanf(fp, "%x", &cache_high);
      fscanf(fp, "%x", &cache_low);
      cache = (cache_high << 16) | (0xffff & cache_low);
      testPIOWrite (bup_bme, 0xe00a8000 + (2*i), cache);
      //printf("addr = %u, cache = %x\n", 4*i, cache);
    }
  fclose(fp);
  printf("\t***** Finished loading fmc_cache\n");
}
//-----------------------------------------------------
void load_dmc_icache_mbup ()
{
  int i, cache_low, cache_high, cache;
  FILE *fp;

  if ((fp = fopen("/tgtsvr/dmc_cpu_icache", "r")) == NULL)
	printf("Error opening file\n");
  for (i=0; i<200; i=i+2)
    {
      fscanf(fp, "%x", &cache_high);
      fscanf(fp, "%x", &cache_low);
      cache = (cache_high << 16) | (0xffff & cache_low);
      testPIOWrite (bup_bme, 0xe00b0000 + (2*i), cache);
      //printf("addr = %u, cache = %x\n", 4*i, cache);
    }
  fclose(fp);  
  printf("\t***** Finished loading dmc_icache\n");
}
//---------------------------------------------
void load_dmc_dcache_mbup (int num_of_ch)
{
  UINT32 addr = 0xe0020140;
  int i=0;

  for (i=0; i<num_of_ch; i++) {
    pio_write( addr, 0x00000000); addr = addr + 4;
    pio_write( addr, 0x00000000); addr = addr + 4;
    pio_write( addr, 0x00ffffff); addr = addr + 4;
    pio_write( addr, 0x35000000); addr = addr + 4;
    pio_write( addr, 0x00000000); addr = addr + 4;
    pio_write( addr, 0x00000000); addr = addr + 4;
    pio_write( addr, 0x00000000); addr = addr + 4;
    pio_write( addr, 0x00000000); addr = addr + 4;
  }
  printf("\t***** Finished loading dmc_dcache\n");
}
//---------------------------------------------
void load_fmc_dcache_mbup (int num_of_ch)
{
  UINT32 addr = 0xe0008140;
  int i=0;

  for (i=0; i<num_of_ch; i++) {
    pio_write( addr, 0x00000000); addr = addr + 4;
    pio_write( addr, 0x00000000); addr = addr + 4;
    pio_write( addr, 0x00003500); addr = addr + 4;
    pio_write( addr, 0xffffff0e); addr = addr + 4;
    pio_write( addr, 0x00000000); addr = addr + 4;
    pio_write( addr, 0x00000000); addr = addr + 4;
    pio_write( addr, 0x00000000); addr = addr + 4;
    pio_write( addr, 0x00000000); addr = addr + 4;
  }
  printf("\t***** Finished loading fmc_dcache\n");
}
//--------------------------------------------------------------
void init_dma ()
{
  // program DMA controller registers
  pio_write( 0xe0030040, 0x80000000);  //ecr
  pio_write( 0xe0030080, 0xc2000000);  //sar
  pio_write( 0xe00300c0, 0xd0000000);  //dar
  pio_write( 0xe0030000, 0x02057000);  //ccr
}
//---------------------------------------------
int start_dma (int DmaSize)
{
  UINT32 temp=0;
  int i=0;
  UINT32 ccr;
  int DmaFail = 0;

  ccr = 0x1601 | ((DmaSize&0x1fff) << 16);
  pio_write( 0xe0030000, ccr);  // start dma
  
  while (!(temp & 0x100)) {
    temp = tprr (bup_bme,0xe0030000);
//    i++;
//    if (i == 200) {
//	printf ("ccr = 0x%x\n",temp);
//    printf ("Error in DMA done poll ********\n");
// 	DmaFail = 1;
//    break;
//    }
  }
}
//---------------------------------------------
void poll_toft (int size)
{
    UINT32 temp, Available, Required;
    UINT32 RdWrap=0;
    UINT32 WrWrap=0;
    UINT32 RdPtr=0;
    UINT32 WrPtr=0;
    UINT32 TempReg=0;
    int Fail = 0;

    Required = size;
    Available = 0x0;
    
        while (Available < Required) {
            TempReg = 0;
            TempReg = tprr (bup_bme, 0xe0048400);
            RdWrap = (TempReg >> 16) & 0x00000001;
            RdPtr  = (TempReg >> 3)  & 0x00001fff;
            WrWrap = (TempReg >> 30) & 0x00000001;
            WrPtr  = (TempReg >> 17) & 0x00001fff;

            if(RdPtr == WrPtr) {
                if(RdWrap == WrWrap)
                    Available = 0;
                else
                    Available = 0x2000;
            }
            else { 
                if(WrPtr > RdPtr)
                    Available = WrPtr - RdPtr ;
                else
                    Available = 0x2000 - RdPtr + WrPtr;
            }
        } 
//	printf ("data=%x, rdw=%x, wrw=%x, rdptr=%x, wrptr=%x, ",TempReg,RdWrap,WrWrap,RdPtr,WrPtr);
    Fail = start_dma (Required-1);
}

//--------------------------------------------------------------
void set_clk () {
    tpw (bup_bme,0xe0070008,0x4f);
    tpw (bup_bme,0xe0070008,0x14f);
    tpw (bup_bme,0xe0070024,0x43);
    tpw (bup_bme,0xe0070024,0x143);
//    printf ("\tCore clock set to 166 Mhz and mem clk to 140 Mhz\n");
}



int  bupJagClrRxReady(int i)
{
	testPIOWrite(1, 0x40008f78, 0);
}




//--------------------------------------------------------------
void bup_diags ()
{
int testnum = 1;
int ptmfprt = 1;
extern int consoleFd;
int i;
int		bytesRead;
int x1, x2, x3;
char buffer [32];
static int	firstTime1 = TRUE;
static int	firstTime2 = TRUE;


//while (1) taskDelay (1);


#ifdef CPE_30
	  bupJagClrRxReady(1);
#else  
	  taskDelay (10);
	  bmeHwResetApi (1);
      taskDelay (10);
	  InitSdramCO(1,1);
      taskDelay (10);
#endif
	  testPIOWrite (1, 0x40000100,0);
	  testPIOWrite (1, 0x40008000,0);
	  testPIOWrite (1, 0x40008004,0);
	  testPIOWrite (1, 0x40008008,0);
	  testPIOWrite (1, 0x4000800c,0);
	  testPIOWrite (1, 0x40008010,0);
      taskDelay (10);


	  set_clk ();
      taskDelay (10);

	  while (1)
	  {
top1:

      printf("\n\t  0. HIC Test\n");
      printf("\t  1. SDRAM Test\n");
      printf("\t  2. EMI Test\n");
      printf("\t  3. DMA Test\n");
      printf("\t  4. UART 0 Write Test\n");
      printf("\t  5. UART 1 Write Test\n");
      printf("\t  6. UART Loop Test\n");
      printf("\t  7. AFE test\n");
      printf("\t  8. IFE test\n");
      printf("\t  9. HIC Stress Test\n");
      printf("\t 10. BME Code Stress test\n");
      printf("\t 20. Display BME Memory '20 address' \n");
      printf("\t 21. Fill BME Memory  '21 address num 32bit_data'\n");
      printf("\t 30. Enter Memory on Board (selected: %x)  '30 memory_size'\n",userMem);
      printf("\t 31. Detect Memory size\n");
#ifndef CPE_30
      printf("\t 40. Enter BME number to test 1,2 (selected %d) '40 number'\n",bup_bme);
#endif

      printf("\n*****************************************************\n");  
top:
	  ioctl (consoleFd, FIORFLUSH, (int) &bytesRead);
	  ioctl (consoleFd, FIONREAD, (int) &bytesRead);
	  printf("Please enter the diag number: ");
	  gets (buffer);
//printf("%s\n",buffer);
	  sscanf (buffer, "%d %x %x %x", &testnum, &x1, &x2, &x3);

	  if (testnum <HIC_STRESS)
	  {
		int data;

	    data = testPIORead1 (bup_bme,0x40000100);
		taskDelay (100);
        if (data == 0x12345678)
		{
			printf("BME Running Code. Please power cycle now...\n");
			while (1);
		}

		bmeHwResetApi (1);
		taskDelay (10);
		taskDelay (100);
	    InitSdramCO(1,1);
	  }
	


	taskDelay (100);

	switch (testnum) {
		case 0: HICTest (); break;
		case 1: SDRAMTest (); break;
		case 2: EMITest (); break;
		case 3: DMATest (); break;
		case 4: UARTTest (0); break;
		case 5: UARTTest (1); break;
		case 6: UARTLoopTest ();break;
		case 7: AFETest ();break;
		case 8: IFETest ();break;
		case HIC_STRESS: HICStressTest ();break;
		case BME_STRESS: BMEStressTest ();break;
		case 20: 
			D (bup_bme, x1,0x40,4);
		break;
		case 21: 
			F (bup_bme, x1,x2,x3);
		break;
		case 30: 
			userMem = x1;
			if (bup_bme == 1)
				userMem1 = userMem;
			else if (bup_bme == 2)
				userMem2 = userMem;
		break;
		case 31: memTestGetMemSz (); break;
#ifndef CPE_30
		case 40: 
			bup_bme = x1;
			if (bup_bme == 1)
				userMem = userMem1;
			else if (bup_bme == 2)
			{
				userMem = userMem2;
				if (firstTime2)
				{
					set_clk ();
					firstTime2 = FALSE;
					testPIOWrite (2, 0x40000100,0);
				}
			}
			InitSdramCO(1,1);

		break;
#endif

		default: 
			ioctl (consoleFd, FIORFLUSH, (int) &bytesRead);
			goto top1;
		break;
	}
	printf("Press ENTER to continue\n");
	getchar ();
    }

}





int SDRAMTest ()
{
int error;

   	error = memTestAddrData ();
//	error |= SdramBasic(); 
	return error;

}

int EMITest ()
{
	return SdramRandom ();
}

int DMATest ()
{
int error, oldP;

	oldP = printDiags;
	printDiags = 0;
	error = SdramDma(1000,0x1a2b3c4d,1,0); 
	if (error)
		printf ("\t**** DMA Test 1 FAILED\n");
	else
		printf ("\tDMA Test 1 Passed\n");
	error |= regress_dma(); 
	if (error)
		printf ("\t**** DMA Test 2 FAILED\n");
	else
		printf ("\tDMA Test 2 Passed\n");
	error |= sdram_dma_1_100bytes(); 
	if (error)
		printf ("\t**** DMA Test 3 FAILED\n");
	else
		printf ("\tDMA Test 3 Passed\n");
	error |= dmadebug();
	if (error)
		printf ("\t**** DMA Test 4 FAILED\n");
	else
		printf ("\tDMA Test 4 Passed\n");
	printDiags = oldP;

	if (error)
	{
		printf ("******************************************************\n"); 
		printf ("\tDMA Test FAILED\n");
		printf ("******************************************************\n"); 
	}
	else if (printDiags)
	{
		printf ("******************************************************\n"); 
		printf ("\tDMA Test PASSED\n");
		printf ("******************************************************\n"); 
	}

	return error;

}

int HICTest ()
{
#ifdef CPE_30
	return check_cpe_id ();
#else
	return check_co_id ();
#endif

}

int UARTTest (int which)
{
	if (which == 0)
		SendMsgUart0CO(); 
	else
		SendMsgUart1CO(); 
	return 0;

}

int UARTLoopTest ()
{
int error;
	error = LoopbackUartCO (0);
	error |= LoopbackUartCO (1);
	return error;
}


int AFETest ()
{
int i;
int error = 0;
int old;

	old = printDiags;
	printDiags = 0;
	printf("\n");

#ifdef CPE_30
	for (i = 0;i < 1000;i++)
	{
		error |= afe_reg_rw();  
		printf(".");
		if (error) break;
	}
#else
	error = afe_reg_rw();  
#endif
	printf("\n");
	printDiags = old;
	return error;
}


int IFETest ()
{
int i = 0;
int error = 0;
int old;

	old = printDiags;
	printDiags = 0;
	printf("\n");

#ifdef CO_20
	for (i = 0;i < 8;i++)
		error |= testIfe (i);
#else
	for (i = 0;i < 1000;i++)
	{
		error |= testIfe (0);
		printf(".");
		if (error) break;
	}
#endif
	printf("\n");
	printDiags = old;
	return error;

}


int BMETest ()
{
int data;

	testHICT2 (); 
	do
	{
		data = testPIORead1 (bup_bme,0x40000104);
		taskDelay (1);
	} while (data == 0x5555);

	if (data == 0x4321)
	{
		if (printDiags) printf("DMA Test passed %x\n", data);
	}
	else
		printf("DMA Test FAILED %x\n", data);

}



int HICStressTest ()
{
#ifdef CPE_30
	bupJagClrRxReady(1);
#endif
	testHICT1 (1000); 
	if (printDiags)
		printf("HIC Test passed\n");
	return 0;
}



int BMEStressTest ()
{
	int data;
	bupJagClrRxReady(1);
	testHICT2 (); 

	do
	{
		data = testPIORead1 (bup_bme,0x40000104);
		taskDelay (1);
	} while (data == 0x5555);

	if (data == 0x4321)
	{
		printf("BME Stress Test passed\n");
		return 0;
	}
	else if (data == 0x1235)
	{
		printf("AFE Stress Test FAILED %x\n", data);
		return 1;
	}
	else 
	{
		printf("BME Stress Test FAILED %x\n", data);
		return 1;
	}


}



/*


		case 0: memTestAddrData ();
		case 1: SdramBasic(); break;
		case 2: SdramDma(1000,0x1a2b3c4d,1,0); break;
		case 3: regress_dma(); break;
		case 4: sdram_dma_1_100bytes(); break;
		case 5: dmadebug(); break;
		case 6: SdramRandom(); break;
		case 7: SdramBurstRefresh(); break;
		case 8: write_read_whole_memory(); break;
		case 9: SendMsgUart0CO(); break;
		case 10: SendMsgUart1CO(); break;
		case 11: LoopbackUartCO(0); break;
		case 12: LoopbackUartCO(1); break;
		case 13: check_CO_id(); break;
		case 14: pio_addr_autoincr_test(); break;
		case 15: 
	   	{
	   		int i;
	   		for (i = 0;i < 8;i++)
	   			afe_all_reg_rw(i); 
			afe_reg_rw();  
			break;
	   	}
		case 16: BasicDmaTestCO(); break;
		case 17: 
		{
			testHICT1 (1000); 
			printf("HIC Test passed\n");
			break;
		}
		case 18: 
		{
			int data;
			testHICT2 (); 

			do
			{
				data = testPIORead1 (1,0x40000104);
				taskDelay (1);
			} while (data == 0x5555);

			if (data == 0x4321)
				printf("DMA Test passed %x\n", data);
			else
				printf("DMA Test FAILED %x\n", data);
			break;
		}

*/
#endif
