/***********************************************************************
*
*             Copyright 2012 Mentor Graphics Corporation
*                         All Rights Reserved.
*
* THIS WORK CONTAINS TRADE SECRET AND PROPRIETARY INFORMATION WHICH IS
* THE PROPERTY OF MENTOR GRAPHICS CORPORATION OR ITS LICENSORS AND IS
* SUBJECT TO LICENSE TERMS.
*
************************************************************************

************************************************************************
*
*   FILE NAME
*
*       arm_mpu.c
*
*   DESCRIPTION
*
*       This file contains the ARM architecture MPU functions
*
*   FUNCTIONS
*
*       ESAL_CO_MEM_Cache_Enable
*       ESAL_CO_MEM_Region_Setup
*       ESAL_CO_MEM_Region_Calc
*
*   DEPENDENCIES
*
*       esal.h                              Embedded Software
*                                           Abstraction Layer external
*                                           interface
*
***********************************************************************/

/* Include required header files */
#include            "nucleus.h"
#include            "os/kernel/plus/core/inc/esal.h"

#if (CFG_NU_OS_ARCH_ARM_COM_MMU_VARIANT == 2)

/* External function prototype. */
extern VOID  ERC_System_Error(INT error_code);

/* Local Function prototypes. */
static VOID    ESAL_CO_MEM_Region_Setup(INT region_num,
                                        UINT32 vrt_addr,
                                        UINT32 phy_addr,
                                        UINT32 size,
                                        ESAL_GE_CACHE_TYPE cache_type,
                                        UINT32 access_type);

static VOID    ESAL_CO_MEM_Region_Calc(UINT32 size, UINT32 phy_addr,
                                       UINT32 *region_size,
                                       UINT32 *phy_addr_aligned,
                                       UINT8  *bit_pos,
                                       UINT8  *sub_reg_dis);

/***********************************************************************
*
*   FUNCTION
*
*       ESAL_CO_MEM_Cache_Enable
*
*   DESCRIPTION
*
*       This function initializes the cache as required
*       for the given core. The memory region data structure
*       (ESAL_DP_MEM_Region_Data) should be utilized to perform
*       this initialization and the cache attributes in this
*       table should be correctly reflected.
*
*   CALLED BY
*
*       ESAL_GE_MEM_Initialize
*
*   CALLS
*
*       ESAL_CO_MEM_Region_Setup
*
*   INPUTS
*
*       avail_mem                           Address of available memory
*
*   OUTPUTS
*
*       VOID *                              Updated available memory
*                                           address
*
***********************************************************************/
VOID    *ESAL_CO_MEM_Cache_Enable(VOID *avail_mem)
{
    INT         region_num;
    UINT32      cp15_ctrl_val;


    /* To avoid compiler warnings. */
    NU_UNUSED_PARAM(avail_mem);

    /* Clear the unaligned memory access bit (A bit - bit 1) in CP15 - C1 control register
       to enable unaligned memory accesses. */
    ESAL_TS_RTE_CP_READ(ESAL_TS_RTE_CP15, 0, &cp15_ctrl_val, ESAL_TS_RTE_C1, ESAL_TS_RTE_C0, 0);

    /* Clear the V bit(13) to set Normal exception vectors range. */
    cp15_ctrl_val &= ~(ESAL_CO_MEM_CP15_CTRL_V);

    /* Clear the alignment bit(1) to enable unaligned memory accesses. */
    cp15_ctrl_val &= ~(ESAL_CO_MEM_CP15_CTRL_A);

    /* The following steps are recommended by Cortex-R4 TRM before enabling/disabling MPU.
       Clean and invalidate all data caches. */
    ESAL_CO_MEM_DCACHE_ALL_FLUSH_INVAL();
    
    /* Disable instruction cache, data cache and MPU. */
    cp15_ctrl_val &= ~(ESAL_CO_MEM_CP15_CTRL_I | ESAL_CO_MEM_CP15_CTRL_C | ESAL_CO_MEM_CP15_CTRL_M | ESAL_CO_MEM_CP15_CTRL_Z);
    ESAL_TS_RTE_DSB_EXECUTE();

    /* Write CP15 System Control Register with updated value. */
    ESAL_TS_RTE_CP_WRITE(ESAL_TS_RTE_CP15, 0, cp15_ctrl_val, ESAL_TS_RTE_C1, ESAL_TS_RTE_C0, 0);
    ESAL_TS_RTE_ISB_EXECUTE();
    
    /* Invalidate all instruction caches. */
    ESAL_CO_MEM_ICACHE_ALL_INVALIDATE();

    /* Read MPU type register to determine number of implemented Unified Memory Regions. */
    ESAL_TS_RTE_CP_READ(ESAL_TS_RTE_CP15, 0, &cp15_ctrl_val, ESAL_TS_RTE_C0, ESAL_TS_RTE_C0, 4);

    /* Return system error if more regions are specified than are available. */
    if (ESAL_DP_MEM_NUM_REGIONS > ((cp15_ctrl_val & ESAL_CO_MEM_MPU_REG_COUNT_MASK) >> 8))
    {
        /* Call System error function. */
        ERC_System_Error(NU_MPU_CONFIG_ERROR);
    }

    /* Walk through all development platform memory regions defined in table. */
    for (region_num = 0; region_num < ESAL_DP_MEM_NUM_REGIONS; region_num++)
    {
        /* Enable caches for this memory section */
        ESAL_CO_MEM_Region_Setup(region_num,
                                (UINT32)ESAL_DP_MEM_Region_Data[region_num].virtual_start_addr,
                                (UINT32)ESAL_DP_MEM_Region_Data[region_num].physical_start_addr,
                                 ESAL_DP_MEM_Region_Data[region_num].size,
                                 ESAL_DP_MEM_Region_Data[region_num].cache_type,
                                 ESAL_DP_MEM_Region_Data[region_num].access_type);

    }   /* for loop */

    /* Read current CP15 control register value. */
    ESAL_TS_RTE_CP_READ(ESAL_TS_RTE_CP15, 0, &cp15_ctrl_val, ESAL_TS_RTE_C1, ESAL_TS_RTE_C0, 0);

    /* Set instruction cache enable / data cache enable / MPU enable bits. */
    cp15_ctrl_val |= (ESAL_CO_MEM_CP15_CTRL_I | ESAL_CO_MEM_CP15_CTRL_C | ESAL_CO_MEM_CP15_CTRL_M);

    /* Enable branch prediction */
    cp15_ctrl_val |=  ESAL_CO_MEM_CP15_CTRL_Z;

    /* Write updated CP15 control register value. */
    ESAL_TS_RTE_CP_WRITE(ESAL_TS_RTE_CP15, 0, cp15_ctrl_val, ESAL_TS_RTE_C1, ESAL_TS_RTE_C0, 0);
    ESAL_TS_RTE_ISB_EXECUTE();

    return (avail_mem);
}

/***********************************************************************
*
*   FUNCTION
*
*       ESAL_CO_MEM_Region_Setup
*
*   DESCRIPTION
*
*       This function sets-up the region of memory based on the given
*       attributes
*
*   CALLED BY
*
*       ESAL_CO_MEM_Cache_Enable
*
*   CALLS
*
*       None
*
*   INPUTS
*
*       region_num                          Number of region begin setup
*       vrt_addr                            Virtual address of region
*       phy_addr                            Physical address of region
*       size                                Size of region
*       cache_type                          Cache type of region
*       access_type                         Memory access type of region
*
*   OUTPUTS
*
*       None
*
***********************************************************************/
static VOID    ESAL_CO_MEM_Region_Setup(INT region_num,
                                        UINT32 vrt_addr,
                                        UINT32 phy_addr,
                                        UINT32 size,
                                        ESAL_GE_CACHE_TYPE cache_type,
                                        UINT32 access_type)
{
    UINT32      access_attr;
    UINT32      region_size;
    UINT32      phy_addr_aligned;
    UINT32      temp32 = 0;
    UINT8       bit_pos;
    UINT8       sub_reg_dis;


    /* Determine MPU region size and base address from input size and physical address. */
    ESAL_CO_MEM_Region_Calc(size, phy_addr, &region_size, &phy_addr_aligned, &bit_pos, &sub_reg_dis);

    /* Set region number. */
    ESAL_TS_RTE_CP_WRITE(ESAL_TS_RTE_CP15, 0, region_num, ESAL_TS_RTE_C6, ESAL_TS_RTE_C2, 0);

    /* Setup Region base address */
    ESAL_TS_RTE_CP_WRITE(ESAL_TS_RTE_CP15, 0, phy_addr_aligned, ESAL_TS_RTE_C6, ESAL_TS_RTE_C1, 0);

    /* If the memory region only contains code. */
    if(access_type == ESAL_DATA)
    {
        /* Set the region access to Read, Write and Execute Never. */
        access_attr = (ESAL_CO_MEM_MPU_AP_FULL | ESAL_CO_MEM_MPU_AP_XN);
    }
    else if (access_type == ESAL_INST)
    {
        /* Set the region access to Read-Only. */
        access_attr = ESAL_CO_MEM_MPU_AP_RO;
    }
    else
    {
        /* Set the region access to Read and Write. */
        access_attr = ESAL_CO_MEM_MPU_AP_FULL;
    }

    /* Check if memory region type is Memory mapped. */
    if (ESAL_DP_MEM_Region_Data[region_num].mem_type != ESAL_MEM_MAPPED)
    {
        /* Determine cache related attributes for MPU Access Control Register. */
        if (cache_type == ESAL_WRITEBACK)
        {
            /* Write-back, no write-allocate. */
            access_attr |= (ESAL_CO_MEM_MPU_ATTR_B | ESAL_CO_MEM_MPU_ATTR_C);
        }
        else if (cache_type == ESAL_WRITETHROUGH)
        {
            /* Write-through, no write-allocate */
            access_attr |= ESAL_CO_MEM_MPU_ATTR_C;
        }
        else if (cache_type == ESAL_NOCACHE)
        {
            /* Normal non-cached memory. */
            access_attr |= ESAL_CO_MEM_MPU_ATTR_TEX;
        }
    }

    /* Setup MPU Region Access Control Register. */
    ESAL_TS_RTE_CP_WRITE(ESAL_TS_RTE_CP15, 0, access_attr, ESAL_TS_RTE_C6, ESAL_TS_RTE_C1, 4);

    /* Determine CP15 register value that determines region size, enables the region,
       disabling extra sub-region to compensate for alignment/size adjustments. */
    temp32 = ((bit_pos - 1) << 1) | (sub_reg_dis << 8) | ESAL_CO_MEM_MPU_REG_ENABLE;

    /* Update CP15 region size and enable register. */
    ESAL_TS_RTE_CP_WRITE(ESAL_TS_RTE_CP15, 0, temp32, ESAL_TS_RTE_C6, ESAL_TS_RTE_C1, 2);
}

/***********************************************************************
*
*   FUNCTION
*
*       ESAL_CO_MEM_Region_Calc
*
*   DESCRIPTION
*
*       Given size and physical address of memory region to be set up, this
*       function returns region size supported by MPU and region base address
*       aligned at region-sized boundary such that corresponding MPU protection
*       region contains entire user input memory region. This function also
*       calculates bit pattern to be programmed into CP15 region size register
*       for disabling extra sub-regions.
*
*   CALLED BY
*
*       ESAL_CO_MEM_Region_Setup
*
*   CALLS
*
*       None
*
*   INPUTS
*
*       phy_addr                            Physical address of region
*       size                                Size of region
*       region_size                         Return pointer to calculated region size
*       phy_addr_aligned                    Return pointer to aligned physical address
*       bit_pos                             Return pointer to set bit position in region_size
*       sub_reg_dis                         Bit pattern to disable extra sub-regions.
*
*   OUTPUTS
*
*       None
*
***********************************************************************/
static VOID    ESAL_CO_MEM_Region_Calc(UINT32 size, UINT32 phy_addr,
                                       UINT32 *region_size,
                                       UINT32 *phy_addr_aligned,
                                       UINT8  *bit_pos,
                                       UINT8  *sub_reg_dis)
{
    UINT32   bit_value;
    UINT32   align_overhead;
    UINT8    index;
    UINT8    count;
    UINT32   sub_reg_size;


    *region_size = size;

    /* Regions sizes less than 32 are not supported. */
    if (size < 32)
    {
        size = 32;
    }

    /* Find largest supported region size less than input size. */
    while (size > 0)
    {
        /* Find rightmost 1 bit. */
        bit_value = ((~size+1) & size);

        /* Clear rightmost 1 bit. */
        size = (size & ~bit_value);
    }

    size = *region_size;

    /* If input size is already supported by MPU. */
    if (bit_value == size)
    {
        *region_size = bit_value;
    }
    /* Else, choose next larger supported region size. */
    else
    {
        *region_size = 2*bit_value;
    }

    /* Align given physical address on region size boundary. */
    align_overhead = phy_addr & (*region_size-1UL);

    /* Find supported region size that accommodates effects of alignment. */
    while ((size + align_overhead) > *region_size)
    {
        *region_size = *region_size*2;

        align_overhead = phy_addr & (*region_size-1UL);
    }

    /* Align given physical address on region size boundary. */
    *phy_addr_aligned = phy_addr & (~(*region_size-1UL));

    *bit_pos = 5;

    /* Determine value to be programmed into CP15 region size register. */
    while (!((*region_size >> *bit_pos)& 0x1))
    {
        *bit_pos = *bit_pos + 1;
    }

    /* Calculate bit pattern that disables extra sub-regions in a MPU region.*/
    if (size >= 256)
    {
        /* A region is divided into eight equal sub-regions. */
        sub_reg_size = *region_size / 8;

        /* Calculate number of sub-regions that makeup alignment overhead. */
        if ((align_overhead % sub_reg_size) == 0)
        {
            index = align_overhead / sub_reg_size;
        }
        else
        {
            index = (align_overhead / sub_reg_size) + 1;
        }

        /* Calculate number of sub-regions that makeup the input region. */
        count = ((size + align_overhead) - index*sub_reg_size) / sub_reg_size;

        /* Calculate bit pattern for disabling extra sub-regions. */
        *sub_reg_dis = ~(((1 << count) - 1) << index);
    }
}

#endif /* (CFG_NU_OS_ARCH_ARM_COM_MMU_VARIANT == 2). */
