#include <NXP/iolpc3131.h>
#include <stdio.h>
#include <string.h>
#include "arm926ej_cp15_drv.h"
#include "arm_comm.h"
#include "drv_cgu.h"
#include "drv_intc.h"
#include "uart.h"
#include "math.h"

#define ISROM_MMU_TTBL  0x1201C000
#define SDRAM_BASE_ADDR 0x30000000
#define SDRAM_SIZE      0x02000000
#define IRAM_BASE_ADDR  0x11028000

#define LED1_SET  {IOCONF_GPIO_M1_SET = (1<<14);IOCONF_GPIO_M0_SET  = (1<<14);}
#define LED1_CLR  {IOCONF_GPIO_M1_SET = (1<<14);IOCONF_GPIO_M0_CLR  = (1<<14);}

#define   TIMER1_IN_FREQ       6MHZ
#define   TIMER1_TICK_MAX      1000   //in miliseconds
#define   TIMER1_TICK_MIN      20     //in miliseconds

#define   TMR1TICK(tick)        ((TIMER1_IN_FREQ/1000)*tick)/256 //Convert miliseconds in Tmr ticks

extern void InitSDRAM(void);
extern void Dly_us(Int32U Dly);
extern Int16U EthernetWalkingOnesTest(Int16U address);
extern Int16U EthernetCheckValueInRegister(Int16U address, Int16U expectedValue);
extern void AssertPeripheralReset(void);
extern void NegatePeripheralReset(void);
extern Int8U Access_PC104(void);

Int32U Tmr1Tick = TMR1TICK(100);

static void SDRAM_Test(void);
static void Test_FPGA_Registers(void);
static void Ethernet_Test(void);
static Int16U WalkingOnesTest(volatile Int16U* address);
static Int16U CheckValueOnReset(volatile Int16U* address, Int16U resetValue);
static void GPIO_RemapTest();
static void PC104_Test();

void Timer1IntrHandler (void)
{
  
  Timer1Load = Tmr1Tick;

  Timer1Clear = 0;              // clear timer interrupt
  
  if(IOCONF_GPIO_M0 & (1<<14))
  {
    LED1_CLR;
  }
  else
  {
    LED1_SET;
  }
}

void main(void)
{
  UartLineCoding_t UartLineCoding;
  char s[128];

  CP15_Mmu(FALSE);            // Disable MMU
  CP15_ICache(TRUE);          // Enable ICache

  InitClock();
  
  InitSDRAM();

  
  /*Init Interrupt Controller. 
    Arm Vector Copy to beginnint of the IRAM*/
  INTC_Init((Int32U *)IRAM_BASE_ADDR);
  /*Remap IRAM at address 0*/
  SYSCREG_ARM926_901616_LP_SHADOW_POINT = IRAM_BASE_ADDR;
  /*Enable Timer 1 Clock*/
  CGU_Run_Clock(TIMER1_PCLK);
  /*Init Timer 1*/
  Timer1Ctrl_bit.Enable = 0;    // disable counting
  Timer1Ctrl_bit.Mode = 1;      // reload mode
  Timer1Ctrl_bit.PreScale = 2;  // prescaler = CGU clock rate / 256
  Timer1Load = Tmr1Tick;
 // set timer 0 period
  Timer1Clear = 0;              // clear timer pending interrupt
  /*Install Interrupt Service Routine, 
    Priority 3*/
  INTC_IRQInstall(Timer1IntrHandler, TIMER0_INTCT1,
                  3,0);
  /*Enable Timer 1 interrupt*/
  INTC_IntEnable(TIMER0_INTCT1, 1);
  
 // Init UART 0
  UartInit(4);
  
  // Update the baud rate
  UartLineCoding.dwDTERate = 115200;
  // Update the stop bits number
  UartLineCoding.bStopBitsFormat = UART_ONE_STOP_BIT;
  // Update the parity type
  UartLineCoding.bParityType = UART_NO_PARITY;
  // Update the word width
  UartLineCoding.bDataBits = 8-5;
  // Set UART line coding
  UartSetLineCoding(UartLineCoding);
  
    __enable_irq();
 
  /*Start Timer 1*/
  Timer1Ctrl_bit.Enable = 1;

  LED1_SET;
   
  SDRAM_Test();
  
  Test_FPGA_Registers();
  
  GPIO_RemapTest();

  Ethernet_Test();
  
  PC104_Test();
  
  sprintf(s,"*********************************************************\n\r");  
  UartWrite((unsigned char *)s,strlen(s)); 
  
  sprintf(s,"All Tests Complete\n\r"); 
  UartWrite((unsigned char *)s,strlen(s));

  while(1);
}

static void SDRAM_Test(void)
{
  char s[64];
  sprintf(s,"*********************************************************\n\r");  
  UartWrite((unsigned char *)s,strlen(s));   
  sprintf(s,"Start of SDRAM Test\n\r");  
  UartWrite((unsigned char *)s,strlen(s));

  /*32bit access test*/
  sprintf(s,"32-bits write\n\r");
  UartWrite((unsigned char *)s,strlen(s));
  /*Start from stram base address*/
  volatile unsigned int * uint_dest = (unsigned int *)SDRAM_BASE_ADDR;
  for(int i = 0; i < SDRAM_SIZE/sizeof(int); i++,uint_dest++)
  {
    /*32-bits write*/
    * uint_dest = i;
  }
  /*32-bits verify*/
  sprintf(s,"32-bits verify\n\r");
  UartWrite((unsigned char *)s,strlen(s));

  uint_dest = (unsigned int *)SDRAM_BASE_ADDR;
  for(int i = 0; i < SDRAM_SIZE/sizeof(int); i++,uint_dest++)
  {
    /*32-bits read*/
    if (* uint_dest != i)
    {
      /*verify error*/
      sprintf(s,"SDRAM 32 Bit Access Test Failed.\n\r");
      UartWrite((unsigned char *)s,strlen(s));
      sprintf(s,"SDRAM 32-bits R/W Error at address %0x\n\r",(unsigned int)uint_dest);
      UartWrite((unsigned char *)s,strlen(s));
      break;
    }
  }
  
  /*16-bits access test*/
  sprintf(s,"16-bits write\n\r");
  UartWrite((unsigned char *)s,strlen(s));
  /*Start from stram base address*/
  volatile unsigned short * ushrt_dest = (unsigned short *)SDRAM_BASE_ADDR;
  for(int i = 0; i < SDRAM_SIZE/sizeof(short); i++,ushrt_dest++)
  {
    /*16-bits write*/
    *ushrt_dest = (i^(i>>16));
  }
  /*16-bits verify*/
  sprintf(s,"16-bits verify\n\r");
  UartWrite((unsigned char *)s,strlen(s));

  ushrt_dest = (unsigned short *)SDRAM_BASE_ADDR;
  for(int i = 0; i < SDRAM_SIZE/sizeof(short); i++,ushrt_dest++)
  {
    /*16-bits read*/
    if ( *ushrt_dest != ((i^(i>>16))&0xFFFF))
    {
      /*verify error*/
      sprintf(s,"SDRAM 16 Bit Acccess Test Failed.\n\r");
      UartWrite((unsigned char *)s,strlen(s));
      sprintf(s,"SDRAM 16-bits R/W Error at address 0x%0x\n\r",(unsigned int)ushrt_dest);
      UartWrite((unsigned char *)s,strlen(s));
      break;
    }
  }
  
  /*8-bits access test*/
  sprintf(s,"8-bits write\n\r");
  UartWrite((unsigned char *)s,strlen(s));
  /*Start from stram base address*/
  volatile unsigned char * uchar_dest = (unsigned char *)SDRAM_BASE_ADDR;
  for(int i = 0; i < SDRAM_SIZE; i++,uchar_dest++)
  {
    /*16-bits write*/
    *uchar_dest = i^(i>>8);
  }
  /*8-bits verify*/
  sprintf(s,"8-bits verify\n\r");
  UartWrite((unsigned char *)s,strlen(s));

  uchar_dest = (unsigned char *)SDRAM_BASE_ADDR;
  for(int i = 0; i < SDRAM_SIZE; i++,uchar_dest++)
  {
    /*8-bits read*/
    if ( *uchar_dest != ((i^(i>>8))&0xFF))
    {
      /*verify error*/
      sprintf(s,"SDRAM 8 Bit Access Test Failed.\n\r");
      UartWrite((unsigned char *)s,strlen(s));
      sprintf(s,"SDRAM 8-bits R/W Error at address %0x\n\r",(unsigned int)ushrt_dest);
      UartWrite((unsigned char *)s,strlen(s));
      break;
    }
  }

  sprintf(s,"SDRAM Test Succeeded.\n\rEnd of SDRAM Test\n\r");  
  UartWrite((unsigned char *)s,strlen(s));
}

static void Ethernet_Test(void)
{
  #define MARL_ADDRESS            0x10
  #define MARM_ADDRESS            0x12
  #define MARH_ADDRESS            0x14
  #define ID_REGISTER_ADDRESS     0xC0
  #define CIDER                   0x8872
  
  char s[64];
  
  AssertPeripheralReset();
  NegatePeripheralReset();
  
  sprintf(s,"*********************************************************\n\r");  
  UartWrite((unsigned char *)s,strlen(s));    
  
  sprintf(s,"Start Ethernet Test\n\r");  
  UartWrite((unsigned char *)s,strlen(s));  
  
  if (EthernetCheckValueInRegister(ID_REGISTER_ADDRESS,CIDER))
    sprintf(s,"Passed: Read of Ethernet Chip Identification Register.\n\r");  
  else
    sprintf(s,"Failed: Read of Ethernet Chip Identification Register.\n\r"); 
  UartWrite((unsigned char *)s,strlen(s)); 
  
  if (EthernetWalkingOnesTest(MARL_ADDRESS))
    sprintf(s,"Failed: Walking One's Test on the Ethernet MARL register.\n\r");  
  else
    sprintf(s,"Passed: Walking One's Test on the Ethernet MARL register.\n\r");   
  UartWrite((unsigned char *)s,strlen(s));
  
  if (EthernetWalkingOnesTest(MARM_ADDRESS))
    sprintf(s,"Failed: Walking One's Test on the Ethernet MARM register.\n\r");  
  else
    sprintf(s,"Passed: Walking One's Test on the Ethernet MARM register.\n\r");   
  UartWrite((unsigned char *)s,strlen(s));
  
  if (EthernetWalkingOnesTest(MARH_ADDRESS))
    sprintf(s,"Failed: Walking One's Test on the Ethernet MARH register.\n\r");  
  else
    sprintf(s,"Passed: Walking One's Test on the Ethernet MARH register.\n\r");   
  UartWrite((unsigned char *)s,strlen(s));
  
  sprintf(s,"End Ethernet Test\n\r");  
  UartWrite((unsigned char *)s,strlen(s));
}

static void Test_FPGA_Registers()
{
  char s[128];
  
  sprintf(s,"*********************************************************\n\r");  
  UartWrite((unsigned char *)s,strlen(s)); 
  sprintf(s,"Start of FPGA Internal Registers Tests\n\r");  
  UartWrite((unsigned char *)s,strlen(s));  
  
  if (CheckValueOnReset((volatile Int16U*) 0x20028000, 0))
    sprintf(s,"Failed: FPGA Special Function Register at 0x20028000 has unexpected reset value.\n\r");  
  else
    sprintf(s,"Passed: FPGA Special Function Register at 0x20028000 has expected reset value.\n\r"); 
  UartWrite((unsigned char *)s,strlen(s)); 
  
  if (CheckValueOnReset((volatile Int16U*) 0x2002BFFE, 0))
    sprintf(s,"Failed: FPGA Special Function Register at 0x2002BFFE has unexpected reset value.\n\r");  
  else
    sprintf(s,"Passed: FPGA Special Function Register at 0x2002BFFE has expected reset value.\n\r"); 
  UartWrite((unsigned char *)s,strlen(s));  
  
//  volatile Int16U crap;
//  while (1) {
//    crap = (*(volatile Int16U*) 0x20020000);  
//    crap = (*(volatile Int16U*) 0x20027FFF);
//    crap = (*(volatile Int16U*) 0x20028000);
//    crap = (*(volatile Int16U*) 0x2002BFFF);
//    crap = (*(volatile Int16U*) 0x2002C000);  
//    crap = (*(volatile Int16U*) 0x2002FFFF);  
//    (*(volatile Int16U*) 0x20038000) = crap;
//  }

  if (CheckValueOnReset((volatile Int16U*) 0x2002C000, 0xCAFE))
    sprintf(s,"Failed: FPGA Test Register at 0x2002C000 has unexpected reset value.\n\r");  
  else
    sprintf(s,"Passed: FPGA Test Register at 0x2002C000 has expected reset value.\n\r"); 
  UartWrite((unsigned char *)s,strlen(s)); 
  
  if (CheckValueOnReset((volatile Int16U*) 0x2002FFFE, 0xCAFE))
    sprintf(s,"Failed: FPGA Test Register at 0x2002FFFE has unexpected reset value.\n\r");  
  else
    sprintf(s,"Passed: FPGA Test Register at 0x2002FFFE has expected reset value.\n\r"); 
  UartWrite((unsigned char *)s,strlen(s));     
  
  if (WalkingOnesTest((volatile Int16U*) 0x20028000) != 0)
    sprintf(s,"Failed: Walking One's Test on FPGA Special Function Register at 0x20028000.\n\r");  
  else
    sprintf(s,"Passed: Walling One's Test on FPGA Special Function Register at 0x20028000.\n\r");   
  UartWrite((unsigned char *)s,strlen(s)); 
  
  if (WalkingOnesTest((volatile Int16U*) 0x2002BFFE) != 0)
    sprintf(s,"Failed: Walking One's Test on FPGA Special Function Register at 0x2002BFFE.\n\r");  
  else
    sprintf(s,"Passed: Walking One's Test on FPGA Special Function Register at 0x2002BFFE.\n\r");   
  UartWrite((unsigned char *)s,strlen(s)); 
  
  if (WalkingOnesTest((volatile Int16U*) 0x2002C000) != 0)
    sprintf(s,"Failed: Walking One's Test on FPGA Test Register at 0x2002C000.\n\r");  
  else
    sprintf(s,"Passed: Walking One's Test on FPGA Test Register at 0x2002C000.\n\r");   
  UartWrite((unsigned char *)s,strlen(s)); 
   
  if (WalkingOnesTest((volatile Int16U*) 0x2002FFFE) != 0)
    sprintf(s,"Failed: Walking One's Test on FPGA Test Register at 0x2002FFFE.\n\r");  
  else
    sprintf(s,"Passed: Walking One's Test on FPGA Test Register at 0x2002FFFE.\n\r");   
  UartWrite((unsigned char *)s,strlen(s)); 
  
  sprintf(s,"End of FPGA Internal Registers Tests\n\r");  
  UartWrite((unsigned char *)s,strlen(s));
}

static Int16U CheckValueOnReset(volatile Int16U* address, Int16U resetValue)
{
  Int16U result;
  
  result = *address;
  if (result != resetValue)
    return result;
  return 0;
}
  
static Int16U WalkingOnesTest(volatile Int16U* address)
{
    Int16U pattern;
    
    // Perform a walking 1's test at the given address.
    for (pattern = 1; pattern != 0; pattern <<= 1)
    {
        // Write the test pattern.
        *address = pattern;
        // Read it back
        if (*address != pattern) 
            return pattern;
    }
    return 0;
}

static Int8U ByteWalkingOnesTest(volatile Int8U* address)
{
    Int8U pattern;
    Int8U data;
    
    // Perform a walking 1's test at the given address.
    for (pattern = 1; pattern != 0; pattern <<= 1)
    {
        // Write the test pattern.
        *address = pattern;
        Dly_us(100);
        // Read it back
        data = *address;
        if (data != pattern) 
            return pattern;
    }
    return 0;
}

static void GPIO_RemapTest()
{
  char s[128];
  volatile Int16U gpio;
  volatile Int16U sfr;
  
  sprintf(s,"*********************************************************\n\r"); 
  UartWrite((unsigned char *)s,strlen(s)); 
  sprintf(s,"Start of GPIO Tests\n\r");  
  UartWrite((unsigned char *)s,strlen(s)); 
  
  sfr = (*(volatile Int16U*) 0x20028000);
  sfr = (sfr & ~1);
  (*(volatile Int16U*) 0x20028000) = sfr;    // Reset Remap Bit
  
  gpio = (*(volatile Int16U*) 0x130031C0);  
  gpio = gpio >> 5;
  gpio = (gpio & 0xFF);
  
  sprintf(s,"GPIO with Remap=0: \t%0o\n\r", gpio);  
  UartWrite((unsigned char *)s,strlen(s));  
  
  sfr = (*(volatile Int16U*) 0x20028000);
  sfr = sfr | 1;
  (*(volatile Int16U*) 0x20028000) = (sfr | 0x0001);    // Set Remap Bit
  
  Dly_us(1);
  
  gpio = (*(volatile Int16U*) 0x130031C0);  
  gpio = gpio >> 5;
  gpio = (gpio & 0xFF);
  
  sprintf(s,"GPIO with Remap=1: \t%0o\n\r", gpio);  
  UartWrite((unsigned char *)s,strlen(s)); 
  
  sprintf(s,"End of GPIO Remap Tests\n\r");  
  UartWrite((unsigned char *)s,strlen(s)); 
}

void PC104_Test()
{
  char s[128];
  sprintf(s,"*********************************************************\n\r"); 
  UartWrite((unsigned char *)s,strlen(s)); 
  sprintf(s,"Start of PC-104 Tests\n\r");  
  UartWrite((unsigned char *)s,strlen(s)); 
  
  sprintf(s, "DVID = \t%x\n\r", Access_PC104());
  UartWrite((unsigned char *) s, strlen(s));
  
  if (ByteWalkingOnesTest((volatile Int8U*) 0x2000048B))
    sprintf(s,"Failed: Walking One's Test on Global Scratch Register at 0x2000048B.\n\r");  
  else
    sprintf(s,"Passed: Walking One's Test on Global Scratch Register at 0x2000048B.\n\r");   
  UartWrite((unsigned char *)s,strlen(s));  
  
  sprintf(s,"End of PC-104 Tests\n\r");  
  UartWrite((unsigned char *)s,strlen(s)); 
}