/*************************************************************************
 *
 *    Used with ICCARM and AARM.
 *
 *    (c) Copyright IAR Systems 2012
 *
 *    File name   : armv7a_cp15_drv.c
 *    Description : Driver for the CP15 of ARMv7-A
 *
 *    History :
 *    1. Date        : September, 8 2006
 *       Author      : Stanimir Bonev
 *       Description : Driver for the ARM926EJ's CP15
 *
 *    2. Date        : October,  2008
 *       Author      : Stoyan Choynev
 *       Description : Port for ARM1136JF. The driver is backwards compatible
 *                     with ARMv5 or earlier processors.
 *
 *    3. Date        : March,  2012
 *       Author      : Atanas Uzunov
 *       Description : Port for ARMv7-A architecture.
 *                     Added cache maintenance functions.
 *
 *    $Revision: 52705 $
 **************************************************************************/

#include "armv7a_cp15_drv.h"

/*************************************************************************
 * Function Name: CP15_GetID
 * Parameters: none
 *
 * Return: Int32U
 *
 * Description: Function returns the ID register
 *
 *************************************************************************/
__arm Int32U CP15_GetID (void)
{
  return(__MRC(15,0,CP15_ID,0,0));
}

/*************************************************************************
 * Function Name: CP15_GetCacheType
 * Parameters: none
 *
 * Return: Int32U
 *
 * Description: Function returns the Cache type
 *
 *************************************************************************/
__arm Int32U CP15_GetCacheType (void)
{
  return(__MRC(15,0,CP15_ID,0,1));
}

/*************************************************************************
 * Function Name: CP15_GetTCM_Status
 * Parameters: none
 *
 * Return: Int32U
 *
 * Description: Function returns the TCM status
 *
 *************************************************************************/
__arm Int32U CP15_GetTCM_Status (void)
{
  return(__MRC(15,0,CP15_ID,0,2));
}

/*************************************************************************
 * Function Name: CP15_GetTtb0
 * Parameters: none
 *
 * Return: Int32U
 *
 * Description: Function returns the TTB0 register
 *
 *************************************************************************/
__arm Int32U CP15_GetTtb0 (void)
{
  return(__MRC(15,0,CP15_TTB_ADDR,0,0));
}

/*************************************************************************
 * Function Name: CP15_GetTtb1
 * Parameters: none
 *
 * Return: Int32U
 *
 * Description: Function returns the TTB1 register
 *
 *************************************************************************/
__arm Int32U CP15_GetTtb1 (void)
{
  return(__MRC(15,0,CP15_TTB_ADDR,0,1));
}

/*************************************************************************
 * Function Name: CP15_GetStatus
 * Parameters: none
 *
 * Return: Int32U
 *
 * Description: Function returns the MMU control register
 *
 *************************************************************************/
__arm Int32U CP15_GetStatus (void)
{
  return(__MRC(15,0,CP15_CTRL,0,0));
}

/*************************************************************************
 * Function Name: CP15_GetDomain
 * Parameters: none
 *
 * Return: Int32U
 *
 * Description: Function returns the MMU domain access register
 *
 *************************************************************************/
__arm Int32U CP15_GetDomain (void)
{
  return(__MRC(15,0,CP15_DA_CTRL,0,0));
}

/*************************************************************************
 * Function Name: CP15_SetDomains
 * Parameters: Int32U DomainAccess
 *
 * Return: Int32U
 *
 * Description: Function set the MMU domain access register
 *
 *************************************************************************/
__arm void CP15_SetDomains (Int32U DomainAccess)
{
register Int32U Val = DomainAccess;
  __MCR(15,0,Val,CP15_DA_CTRL,0,0);
}

/*************************************************************************
 * Function Name: log2_n_up
 * Parameters: Int32U n
 *
 * Return: Int32S
 *
 * Description: Logarithm at base 2 , rounded up
 *
 *************************************************************************/
Int32S log2_up(Int32U n)
{
  Int32S log = -1;
  Int32U t = n;
  while(t)
  {
    log++; t >>=1;
  }
  /* if n not power of 2 -> round up*/
  if ( n & (n - 1) ) log++;
  return log;
}

/*************************************************************************
 * Function Name: CP15_MaintainDCacheSetWay
 * Parameters: Int32U level - level of cache, 
 *             Int32U maint - maintenance type
 *
 * Return: none
 *
 * Description: Maintain data cache line by Set/Way
 *
 *************************************************************************/
__arm void CP15_MaintainDCacheSetWay(Int32U level, Int32U maint)
{
register volatile Int32U Dummy;
register volatile Int32U ccsidr;
Int32U num_sets;
Int32U num_ways;
Int32U shift_way;
Int32U log2_linesize;
Int32U log2_num_ways;

  Dummy = level << 1;
  /* set csselr, select ccsidr register */
  __MCR(15,2,Dummy,0,0,0);
  /* get current ccsidr register */
  ccsidr = __MRC(15,1,0,0,0);
  num_sets = ((ccsidr & 0x0FFFE000) >> 13) + 1;
  num_ways = ((ccsidr & 0x00001FF8) >> 3) + 1;
  log2_linesize = (ccsidr & 0x00000007) + 2 + 2;
  log2_num_ways = log2_up(num_ways);
  shift_way = 32 - log2_num_ways;
  for(int way = num_ways-1; way >= 0; way--)
    for(int set = num_sets-1; set >= 0; set--)
    {
      Dummy = (level << 1) | (set << log2_linesize) | (way << shift_way);
      switch (maint)
      {
        case DCACHE_CLEAN_AND_INVALIDATE:
             __MCR(15,0,Dummy,7,14,2);
             break;
        
        case DCACHE_INVALIDATE:
             __MCR(15,0,Dummy,7,6,2);
             break;
      }
    }
    __DMB();
}

/*************************************************************************
 * Function Name: CP15_MaintAllDCache
 * Parameters: Int32U oper - type of maintenance, one of:
 *                          DCACHE_CLEAN_AND_INVALIDATE
 *                          DCACHE_INVALIDATE
 *
 * Return: none
 *
 * Description: Maintenance of all data cache
 *
 *************************************************************************/
__arm void CP15_MaintainAllDCache(Int32U oper)
{
register volatile Int32U clidr;  
Int32U cache_type;
  clidr =  __MRC(15,1,0,0,1);
  for(Int32U i = 0; i<7; i++)
  {
    cache_type = (clidr >> i*3) & 0x7UL;
    if ((cache_type >= 2) && (cache_type <= 4))
    {
      CP15_MaintainDCacheSetWay(i,oper);
    }
  }
}

/*************************************************************************
 * Function Name: CP15_InvalInstrCache
 * Parameters: none
 *
 * Return: none
 *
 * Description: Invalidate instruction cache
 *
 *************************************************************************/
__arm void CP15_InvalInstrCache(void)
{
register volatile Int32U Dummy;
  __MCR(15,0,Dummy,CP15_CACHE_OPR,5,0);
  CP15_InvalPredictArray();
  __DSB();
  __ISB();
}

/*************************************************************************
 * Function Name: CP15_InvalPredictArray
 * Parameters: none
 *
 * Return: none
 *
 * Description: Invalidate prediction array
 *
 *************************************************************************/
__arm void CP15_InvalPredictArray(void)
{
register volatile Int32U Dummy;
  __MCR(15,0,Dummy,CP15_CACHE_OPR,5,6);  __ISB();
}

/*************************************************************************
 * Function Name: CP15_InvalAllTbl
 * Parameters: none
 *
 * Return: none
 *
 * Description: Invalidate TLB
 *
 *************************************************************************/
__arm void CP15_InvalAllTbl (void)
{
register volatile Int32U Dummy;
  /* Invalidate entire unified TLB*/
  __MCR(15,0,Dummy,CP15_TBL_OPR,7,0);
  /* Invalidate entire data TLB*/
  __MCR(15,0,Dummy,CP15_TBL_OPR,6,0);
  /* Invalidate entire instruction TLB*/
  __MCR(15,0,Dummy,CP15_TBL_OPR,5,0);
  __DSB();
  __ISB();
}

/*************************************************************************
 * Function Name: CP15_SetStatus
 * Parameters: Int32U Ctrl
 *
 * Return: none
 *
 * Description: Set CP15 CTR (control) register
 *
 *************************************************************************/
__arm void CP15_SetStatus (Int32U Ctrl)
{
register volatile Int32U Val = Ctrl;
  __MCR(15,0,Val,CP15_CTRL,0,0);
}

/*************************************************************************
 * Function Name: CP15_SetTtb0
 * Parameters: pInt32U pTtb
 *
 * Return: none
 *
 * Description: Set CP15 TTB0 base address register
 *
 *************************************************************************/
__arm void CP15_SetTtb0 (pInt32U pTtb)
{
register volatile Int32U Val = (Int32U)pTtb;
  __MCR(15,0,Val,CP15_TTB_ADDR,0,0);
}

/*************************************************************************
 * Function Name: CP15_SetTtb1
 * Parameters: pInt32U pTtb
 *
 * Return: none
 *
 * Description: Set CP15 TTB1 base address register
 *
 *************************************************************************/
__arm void CP15_SetTtb1 (pInt32U pTtb)
{
register volatile Int32U Val = (Int32U)pTtb;
  __MCR(15,0,Val,CP15_TTB_ADDR,0,1);
}

/*************************************************************************
 * Function Name: CP15_SetDac
 * Parameters: Int32U da
 *
 * Return: none
 *
 * Description: Set CP15 domain access register
 *
 *************************************************************************/
__arm void CP15_SetDac (Int32U da)
{
register volatile Int32U Val = da;
  __MCR(15,0,Val,CP15_DA_CTRL,0,0);
}

/*************************************************************************
 * Function Name: CP15_WriteBuffFlush
 * Parameters: none
 *
 * Return: none
 *
 * Description:  Flush the write buffer and wait for completion
 *              of the flush.
 *
 *************************************************************************/
__arm void CP15_WriteBuffFlush (void)
{
register volatile Int32U Val;
  __MCR(15,0,Val,CP15_CACHE_OPR,10,4);
}

/*************************************************************************
 * Function Name: CP15_GetFaultStat
 * Parameters: none
 *
 * Return: Int32U
 *
 * Description: Function returns the MMU fault status register
 *
 *************************************************************************/
__arm Int32U CP15_GetFaultStat (void)
{
  return(__MRC(15,0,CP15_FAULT_STAT,0,0));
}

/*************************************************************************
 * Function Name: CP15_GetFaultAddr
 * Parameters: none
 *
 * Return: Int32U
 *
 * Description: Function returns the MMU fault address register
 *
 *************************************************************************/
__arm Int32U CP15_GetFaultAddr (void)
{
  return(__MRC(15,0,CP15_FAULT_ADDR,0,0));
}

/*************************************************************************
 * Function Name: CP15_GetFcsePid
 * Parameters: none
 *
 * Return: Int32U
 *
 * Description: Function returns the MMU Process identifier
 *             FCSE PID register
 *
 *************************************************************************/
__arm Int32U CP15_GetFcsePid (void)
{
  return(__MRC(15,0,CP15_PROCESS_IDNF,0,0));
}

/*************************************************************************
 * Function Name: CP15_GetPraceProcId
 * Parameters: none
 *
 * Return: Int32U
 *
 * Description: Function returns the MMU Trace Process identifier
 *             register
 *
 *************************************************************************/
__arm Int32U CP15_GetPraceProcId (void)
{
  return(__MRC(15,0,CP15_PROCESS_IDNF,0,1));
}

/*************************************************************************
 * Function Name: CP15_SetFcsePid
 * Parameters: Int32U FcsePid
 *
 * Return: none
 *
 * Description: Function set the MMU Process identifier
 *             FCSE PID register
 *
 *************************************************************************/
__arm void CP15_SetFcsePid (Int32U FcsePid)
{
register Int32U Val = FcsePid;
  __MCR(15,0,Val,CP15_PROCESS_IDNF,0,0);
}

/*************************************************************************
 * Function Name: CP15_GetPraceProcId
 * Parameters: Int32U
 *
 * Return: none
 *
 * Description: Function set the MMU Trace Process identifier
 *             register
 *
 *************************************************************************/
__arm void CP15_SetPraceProcId(Int32U Trace)
{
register Int32U Val = Trace;
  __MCR(15,0,Val,CP15_PROCESS_IDNF,0,1);
}

/*************************************************************************
 * Function Name: CP15_InitMmuTtb
 * Parameters: pTtSectionBlock_t pTtSB, pTtTableBlock_t pTtTB
 *
 * Return: Boolean
 *
 *  Returns error if MMU is enabled or if target
 * Translation Table address is not 16K aligned. Clear the
 * Translation Table area. Build the Translation Table from the
 * initialization data in the Section Block array. Return no error.
 *
 * Description:  Initializes the MMU tables.
 *
 *
 *************************************************************************/
Boolean CP15_InitMmuTtb(const TtSectionBlock_t * pTtSB,
                        const TtTableBlock_t * pTtTB)
{
Int32U i, pa, pa_inc, va_ind;
pInt32U pTtb;
TableType_t TableType;
  while(1)
  {
    TableType = pTtTB->TableType;
    switch(TableType)
    {
    case TableL1:
      pTtb = pTtTB->TableAddr;
      if((Int32U)pTtb & L1_ENTRIES_NUMB-1)
      {
        return(FALSE);
      }
      pa_inc = 0x100000;
      pa = L1_ENTRIES_NUMB;
      break;
    case TableL2_PageTable:
      pTtb = pTtTB->TableAddr;
      if((Int32U)pTtb & L2_CP_ENTRIES_NUMB-1)
      {
        return(FALSE);
      }
      pa_inc = 0x1000;
      pa = L2_CP_ENTRIES_NUMB;
      break;
    default:
      return(TRUE);
    }

    // Clear the entire Translation Table This results in LxD_TYPE_FAULT
    // being the default for any uninitialized entries.
    for(i = 0; i < pa; ++i)
    {
      *(pTtb+i) = TT_ENTRY_INVALID;
    }

    // Build the translation table from user provided pTtSectionBlock_t array
    while(pTtSB->NubrOfSections != 0)
    {
Int32U Entrys = pTtSB->NubrOfSections;
Int32U Data =  pTtSB->Entry.Data;   
      pa = pTtSB->PhysAddr;
      
      switch(TableType)
      {
      case TableL1:
        va_ind = (pTtSB->VirtAddr >> 20) & (L1_ENTRIES_NUMB-1);
                
        if((va_ind + Entrys) > L1_ENTRIES_NUMB)
        {
          return(FALSE);
        }
        break;
      case TableL2_PageTable:
        va_ind = (pTtSB->VirtAddr >> 12) & (L2_CP_ENTRIES_NUMB-1);
        if((va_ind + Entrys) > L2_CP_ENTRIES_NUMB)
        {
          return(FALSE);
        }
        break;
      }
      for(i = 0; i < Entrys; ++i, ++va_ind)
      {
        switch(TableType)
        {
        case TableL1:
          switch(pTtSB->Entry.Type)
          {
          case TtL1PageTable:
            *(pTtb+va_ind) |= Data | (pa & TTL1_PT_PADDR_MASK);
            break;
          case TtL1Section:
            *(pTtb+va_ind) |= Data | (pa & TTL1_SECTION_PADDR_MASK);
            break;
          case TtL1SuperSection:
              *(pTtb+va_ind) |= Data | (pa & TTL1_S_SECTION_PADDR_MASK);
            break;
          default:
            return(FALSE);
          }
          break;
        case TableL2_PageTable:
          switch(pTtSB->Entry.Type)
          {
          case TtL2LargePage:
            *(pTtb+va_ind) |= Data | (pa & TTL2_LP_PADDR_MASK);
            break;
          case TtL2SmallPage:
            *(pTtb+va_ind) |= Data | (pa & TTL2_SP_PADDR_MASK);
            break;
          default:
            return(FALSE);
          }
          break;
        }
        pa += pa_inc;
      }
      ++pTtSB;
    }
    ++pTtSB;
    ++pTtTB;
  }
}

/*************************************************************************
 * Function Name: CP15_Mmu
 * Parameters: Boolean Enable
 *
 * Return: none
 *
 * Description: Enable/Disable MMU
 *
 *************************************************************************/
void CP15_Mmu(Boolean Enable)
{
Int32U Val = CP15_GetStatus();
  if(Enable)
  {
    CP15_InvalAllTbl();
    Val |= CP15_CTRL_M;
  }
  else
  {
    Val &= ~(CP15_CTRL_M | CP15_CTRL_C);
  }  
  CP15_SetStatus(Val);
}

/*************************************************************************
 * Function Name: CP15_Cache
 * Parameters: Boolean Enable
 *
 * Return: none
 *
 * Description: Enable/Disable Both Cache
 *
 *************************************************************************/
void CP15_Cache(Boolean Enable)
{
Int32U Val = CP15_GetStatus();
  if(Enable)
  { 
    Val |= CP15_CTRL_M | CP15_CTRL_C | CP15_CTRL_I;
  }
  else
  {
    Val &= ~CP15_CTRL_C;
  }
  CP15_SetStatus(Val);  
}

/*************************************************************************
 * Function Name: CP15_InvalidateCache
 * Parameters: Boolean Enable
 *
 * Return: none
 *
 * Description: Invalidate Cache
 *
 *************************************************************************/
void CP15_InvalidateCache()
{
  CP15_MaintainAllDCache(DCACHE_INVALIDATE);
  __DSB();
  CP15_InvalInstrCache(); /* includes invalidation of branch predictor */
  __DSB();
  __ISB();
}

/*************************************************************************
 * Function Name: CP15_ICache
 * Parameters: Boolean Enable
 *
 * Return: none
 *
 * Description: Enable/Disable I cache
 *
 *************************************************************************/
void CP15_ICache (Boolean Enable)
{
Int32U Val = CP15_GetStatus();
  if(Enable)
  {
    Val |= CP15_CTRL_I;
  }
  else
  {
    Val &= ~CP15_CTRL_I;
  }
  CP15_SetStatus(Val);
}

/*************************************************************************
 * Function Name: CP15_DCache
 * Parameters: Boolean Enable
 *
 * Return: none
 *
 * Description: Enable/Disable D cache
 *
 *************************************************************************/
void CP15_DCache (Boolean Enable)
{
Int32U Val = CP15_GetStatus();
  if(Enable)
  {
    Val |= CP15_CTRL_M | CP15_CTRL_C;
  }
  else
  {
    Val &= ~CP15_CTRL_C;
  }
  CP15_SetStatus(Val);
}

/*************************************************************************
 * Function Name: CP15_ProgFlowPrediction
 * Parameters: Boolean Enable
 *
 * Return: none
 *
 * Description: Enable/Disable program flow prediction.
 *
 *************************************************************************/
void CP15_ProgFlowPrediction (Boolean Enable)
{
Int32U Val = CP15_GetStatus();
  if(Enable)
  {
    CP15_InvalPredictArray();
    Val |= CP15_CTRL_Z;
  }
  else
  {
    Val &= ~CP15_CTRL_Z;
  }
  CP15_SetStatus(Val);
}

/*************************************************************************
 * Function Name: CP15_GetVectorBase
 * Parameters: none
 *
 * Return: Int32U
 *
 * Description: Get Vector Base Register (VBAR)
 *
 *************************************************************************/
__arm Int32U CP15_GetVectorBase(void)
{
  return(__MRC(15,0,CP15_VBAR,0,0));
}

/*************************************************************************
 * Function Name: CP15_SetVectorBase
 * Parameters: Int32U
 *
 * Return: none
 *
 * Description: Set Vector Base Register (VBAR)
 *
 *************************************************************************/
__arm void CP15_SetVectorBase(Int32U vector)
{
register volatile Int32U Val = vector;
  __MCR(15,0,Val,CP15_VBAR,0,0);
}

/*************************************************************************
 * Function Name: CP15_SetHighVectors
 * Parameters: Boolean
 *
 * Return: none
 *
 * Description: Select High or Low vectors base in CP15 control register
 *
 *************************************************************************/
__arm void CP15_SetHighVectors(Boolean Enable)
{
Int32U Val = CP15_GetStatus();
  if(Enable)
  {
    Val |= CP15_CTRL_V;
  }
  else
  {
    Val &= ~CP15_CTRL_V;
  }
  CP15_SetStatus(Val);
}