#include "common.h"

/* The CP15 "ID" register (c0) contains the processor name and revision. */
U32 ARM_CP15_DeviceIDRead(void)
{
  U32 id;
  __asm { MRC P15, 0, id, c0, c0; }
  return id;
}

void ARM_CP15_SetPageTableBase(P_U32 TableAddress)
{
   __asm { MCR  P15, 0, TableAddress, c2, c0, 0; }
}

void ARM_CP15_SetDomainAccessControl(U32 flags)
{
  __asm { MCR  P15, 0, flags, c3, c0, 0; }
}

void ARM_CP15_ICacheFlush()
{
  unsigned long dummy;

  __asm { MCR p15, 0, dummy, c7, c5, 0; }
}

void ARM_CP15_DCacheFlush()
{
  unsigned long dummy;

  __asm { MCR p15, 0, dummy, c7, c6, 0; }
}

void ARM_CP15_CacheFlush()
{
  unsigned long dummy;

  __asm { MCR p15, 0, dummy, c7, c7, 0; }
}

void ARM_CP15_TLBFlush(void)
{
  unsigned long dummy;

  __asm { MCR  P15, 0, dummy, c8, c7, 0; }
}

void ARM_CP15_ControlRegisterWrite(U32 flags)
{
  __asm { MCR P15, 0, flags, c1, c0; }
}

void ARM_CP15_ControlRegisterOR(U32 flag)
{

__asm {
    mrc p15,0,r0,c1,c0,0
    mov r2,flag
    orr r0,r2,r0
    mcr p15,0,r0,c1,c0,0
}

}

void ARM_CP15_ControlRegisterAND(U32 flag)
{

__asm {
    mrc p15,0,r0,c1,c0,0
    mov r2,flag
    and r0,r2,r0
    mcr p15,0,r0,c1,c0,0
}

}

void ARM_MMU_Init(P_U32 TableAddress)
{
	ARM_CP15_TLBFlush();
    ARM_CP15_CacheFlush();
    ARM_CP15_SetDomainAccessControl(0xFFFFFFFF);
    ARM_CP15_SetPageTableBase(TableAddress);
}