#include "ks8695px.h"
#include "bus_control_register.h"
#include "external_io_registers.h"
#include "watchdog.h"
#include "timer_registers.h"

const uint32_t	MMU_Mask		=	0x00000001;
const uint32_t	DCacheMask		=	0x00000004;
const uint32_t	ICacheMask		=	0x00001000;
const uint32_t	FCSEMask		=	0xC0000000;

void CPU::SetSystemClockSpeed(CLOCK_SPEED speed)
{
	regWrite(BUS_CONTROL::CLKCON, static_cast<uint32_t>(speed));
}

void CPU::Reset(void)
{
	const uint32_t Timer_0_Enable	= (1 << 0);
	
	regWrite(TIMERS::T0TC, 0x1FF);
	regWrite(TIMERS::T0PD, 0);
	regWrite(TIMERS::TMCON, (regRead(TIMERS::TMCON) |  Timer_0_Enable));
	// Wait until watchdog resets CPU
	for (;;) ;
}
                  
void CPU::InitializeChipSelectZero(uint32_t 	baseAddress, 
							       uint32_t 	size,
							  	   BUS_WIDTH  	width,
                              	   CYCLES 		time_OE_or_WE_Asserted,
							  	   CYCLES 		addressHoldTimeAfter_CS_Negated, 
							       CYCLES 		addressSetupTimeBefore_CS_Asserted,
						           CYCLES 		addressSetupTimeBefore_OE_or_WE_Asserted)
{
	bitWrite(EXTERNAL_IO::ERGCON, 	EXTERNAL_IO::BUS_WIDTH_FOR_EXTERNAL_BANK_0, width);
	bitWrite(EXTERNAL_IO::EXTACON0, EXTERNAL_IO::LAST_ADDRESS, ((baseAddress + size - 1) >> 16));
	bitWrite(EXTERNAL_IO::EXTACON0, EXTERNAL_IO::BASE_ADDRESS, (baseAddress >> 16));
	bitWrite(EXTERNAL_IO::EXTACON0, EXTERNAL_IO::TIME_OE_OR_WE_IS_ASSERTED, 
	           time_OE_or_WE_Asserted);
	bitWrite(EXTERNAL_IO::EXTACON0, EXTERNAL_IO::CS_HOLD_TIME_AFTER_OE_OR_WE, 
	           addressHoldTimeAfter_CS_Negated);
	bitWrite(EXTERNAL_IO::EXTACON0, EXTERNAL_IO::ADDRESS_SETUP_TIME_BEFORE_CS, 
	           addressSetupTimeBefore_CS_Asserted );
	bitWrite(EXTERNAL_IO::EXTACON0, EXTERNAL_IO::CS_SETUP_TIME_BEFORE_OE_OR_WE, 
	           addressSetupTimeBefore_OE_or_WE_Asserted );
}

void CPU::InitializeChipSelectOne(uint32_t 		baseAddress, 
							      uint32_t 		size,
							  	  BUS_WIDTH  	width,
                              	  CYCLES 		time_OE_or_WE_Asserted,
							  	  CYCLES 		addressHoldTimeAfter_CS_Negated, 
							      CYCLES 		addressSetupTimeBefore_CS_Asserted,
						          CYCLES 		addressSetupTimeBefore_OE_or_WE_Asserted)
{
	bitWrite(EXTERNAL_IO::ERGCON,   EXTERNAL_IO::BUS_WIDTH_FOR_EXTERNAL_BANK_1, width);
	bitWrite(EXTERNAL_IO::EXTACON1, EXTERNAL_IO::LAST_ADDRESS, ((baseAddress + size - 1) >> 16));
	bitWrite(EXTERNAL_IO::EXTACON1, EXTERNAL_IO::BASE_ADDRESS, (baseAddress >> 16));
	bitWrite(EXTERNAL_IO::EXTACON1, EXTERNAL_IO::TIME_OE_OR_WE_IS_ASSERTED, 
	           time_OE_or_WE_Asserted);
	bitWrite(EXTERNAL_IO::EXTACON1, EXTERNAL_IO::CS_HOLD_TIME_AFTER_OE_OR_WE, 
	           addressHoldTimeAfter_CS_Negated);
	bitWrite(EXTERNAL_IO::EXTACON1, EXTERNAL_IO::ADDRESS_SETUP_TIME_BEFORE_CS, 
	           addressSetupTimeBefore_CS_Asserted );
	bitWrite(EXTERNAL_IO::EXTACON1, EXTERNAL_IO::CS_SETUP_TIME_BEFORE_OE_OR_WE, 
	           addressSetupTimeBefore_OE_or_WE_Asserted );
}

void CPU::InitializeChipSelectTwo(uint32_t 		baseAddress, 
							      uint32_t 		size,
							  	  BUS_WIDTH  	width,
                              	  CYCLES 		time_OE_or_WE_Asserted,
							  	  CYCLES 		addressHoldTimeAfter_CS_Negated, 
							      CYCLES 		addressSetupTimeBefore_CS_Asserted,
						          CYCLES 		addressSetupTimeBefore_OE_or_WE_Asserted)
{
	bitWrite(EXTERNAL_IO::ERGCON, 	EXTERNAL_IO::BUS_WIDTH_FOR_EXTERNAL_BANK_2, width);
	bitWrite(EXTERNAL_IO::EXTACON2, EXTERNAL_IO::LAST_ADDRESS, ((baseAddress + size - 1) >> 16));
	bitWrite(EXTERNAL_IO::EXTACON2, EXTERNAL_IO::BASE_ADDRESS,  (baseAddress >> 16));
	bitWrite(EXTERNAL_IO::EXTACON2, EXTERNAL_IO::TIME_OE_OR_WE_IS_ASSERTED, 
	           time_OE_or_WE_Asserted);
	bitWrite(EXTERNAL_IO::EXTACON2, EXTERNAL_IO::CS_HOLD_TIME_AFTER_OE_OR_WE, 
	           addressHoldTimeAfter_CS_Negated);
	bitWrite(EXTERNAL_IO::EXTACON2, EXTERNAL_IO::ADDRESS_SETUP_TIME_BEFORE_CS, 
	           addressSetupTimeBefore_CS_Asserted );
	bitWrite(EXTERNAL_IO::EXTACON2, EXTERNAL_IO::CS_SETUP_TIME_BEFORE_OE_OR_WE, 
	           addressSetupTimeBefore_OE_or_WE_Asserted );
}

void CPU::EnableMMU(void)
{
	setCoprocessorRegisterBits(MMU_Mask);
}

void CPU::EnableICache(void)
{
	setCoprocessorRegisterBits(ICacheMask);
}

void CPU::EnableDCache(void)
{
	setCoprocessorRegisterBits(DCacheMask);
}

void CPU::EnableFastContextSwitch(void)
{
	setCoprocessorRegisterBits(FCSEMask);
}

void CPU::DisableMMU(void)
{
	clearCoprocessorRegisterBits(MMU_Mask);
}

void CPU::DisableICache(void)
{
	clearCoprocessorRegisterBits(ICacheMask);
}

void CPU::DisableDCache(void)
{
	clearCoprocessorRegisterBits(DCacheMask);
}

void CPU::DisableFastContextSwitch(void)
{
	clearCoprocessorRegisterBits(FCSEMask);
}

void CPU::setCoprocessorRegisterBits(uint32_t mask)
{	
	uint32_t c1format;
   	asm volatile ("  MRC      p15, 0, r0, c1, c0 ,0"
   	              : "=r"(c1format)
   	              );
   	              
   	c1format = c1format | mask;
   	
	asm volatile ("  MCR      p15, 0, r0, c1, c0 ,0"
            	  :
                  : "r" (c1format)
                  );
}

void CPU::clearCoprocessorRegisterBits(uint32_t mask)
{	
	uint32_t c1format;
   	asm volatile ("  MRC      p15, 0, r0 , c1, c0 ,0"
   	              : "=r"(c1format)
   	              );
   	c1format = c1format & ~mask;
   	
	asm volatile ("  MCR      p15, 0, r0, c1, c0 ,0"
            	  :
            	  : "r" (c1format)
            	  );
}