/*******************************************************************************
* Freescale Semiconductor Inc.
* (c) Copyright 2014 Freescale Semiconductor, Inc.
* ALL RIGHTS RESERVED.
********************************************************************************
Services performed by FREESCALE in this matter are performed AS IS and without 
any warranty. CUSTOMER retains the final decision relative to the total design 
and functionality of the end product. FREESCALE neither guarantees nor will be 
held liable by CUSTOMER for the success of this project.
FREESCALE DISCLAIMS ALL WARRANTIES, EXPRESSED, IMPLIED OR STATUTORY INCLUDING, 
BUT NOT LIMITED TO, IMPLIED WARRANTY OF MERCHANTABILITY OR FITNESS FOR 
A PARTICULAR PURPOSE ON ANY HARDWARE, SOFTWARE ORE ADVISE SUPPLIED TO THE PROJECT
BY FREESCALE, AND OR NAY PRODUCT RESULTING FROM FREESCALE SERVICES. IN NO EVENT
SHALL FREESCALE BE LIABLE FOR INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF 
THIS AGREEMENT.

CUSTOMER agrees to hold FREESCALE harmless against any and all claims demands or 
actions by anyone on account of any damage, or injury, whether commercial, 
contractual, or tortuous, rising directly or indirectly as a result of the advise
or assistance supplied CUSTOMER in connection with product, services or goods 
supplied under this Agreement.
********************************************************************************
* File              Optimizations.c
* Owner             b05111
* Version           0.0
* Date              Nov-14-2014
* Classification    General Business Information
********************************************************************************
* Detailed Description:
********************************************************************************
Revision History:
Version  Date         Author        Description of Changes
0.0      Nov-14-2014  b05111        Initial version
*******************************************************************************/

#include "Optimizations.h"
#include "MPC5675K.h"

/*******************************************************************************
* Local function prototypes
*******************************************************************************/

static asm void ICache_init(void);
static asm void DCache_init(void);
static asm void Enable_DCache_Copyback(void);
static asm void MMUFlashIsCacheInhibited(void);
static asm void MMUFlashIsCacheable(void);
static asm void MMUSRAMIsCacheable(void);
static asm void BranchPredictionEnable(void);


/*******************************************************************************
* Local functions
*******************************************************************************/

/*******************************************************************************
Function Name : MMUFlashIsCacheInhibited
Engineer      : b05111
Date          : Nov-14-2014
Parameters    : 
Modifies      : r3, TLB1_ENTRY0
Returns       : 
Notes         : modify TLB1_ENTRY0 to be cache inhibited.                 
Issues        : TLB1_ENTRY0 is supposed to be Internal Flash.
*******************************************************************************/
static asm void MMUFlashIsCacheInhibited(void)
{
    nofralloc
    
    lis    r3, 0x1000    /* Select TLB entry #, define RW replacement control */
    mtMAS0 r3            /* Load MAS0 with 0x1000 0000 for TLB1 entry #0 */

    tlbre                /* Get TLB entry # information */
  
    mfspr  r3, MAS2
    ori	 r3, r3, 0x0008  /* set I bit (Page is cache inhibited) */
	                      
    mtMAS2 r3
  
    msync        /* make sure we finished all memory accesses */
  
    tlbwe        /* Write entry defined in MAS0 (entry 0 here) to MMU TLB1 */
  
    isync        /* Wait for tlbwe to complete, then flush instruction buffer */
    
    blr
}


/*******************************************************************************
Function Name : MMUFlashIsCacheable
Engineer      : b05111
Date          : Nov-14-2014
Parameters    : 
Modifies      : r3, TLB1_ENTRY0
Returns       : 
Notes         : modify TLB1_ENTRY0 to be cacheable.                  
Issues        : TLB1_ENTRY0 is supposed to be Internal Flash.
*******************************************************************************/
static asm void MMUFlashIsCacheable(void)
{
    nofralloc
    
    lis    r3, 0x1000     /* Select TLB entry #, define RW replacement control */
    mtMAS0 r3             /* Load MAS0 with 0x1000 0000 for TLB1 entry #0 */

    tlbre                 /* Get TLB entry # information */
  
    mfspr   r3, MAS2
    lis     r4, 0xFFFF
    ori     r4, r4, 0xFFF7    
    and     r3, r3, r4    /* clear I bit (cacheable) */
	                      
    mtMAS2 r3
  
    msync        /* make sure we finished all memory accesses */
  
    tlbwe        /* Write entry defined in MAS0 (entry 1 here) to MMU TLB */
  
    isync        /* Wait for tlbwe to complete, then flush instruction buffer */
    blr    
}


/*******************************************************************************
Function Name : MMUSRAMIsCacheable
Engineer      : b05111
Date          : Nov-14-2014
Parameters    : 
Modifies      : r3, TLB1_ENTRY2 (+TLB1_ENTRY3 in DPM)
Returns       : 
Notes         : modify TLB1_ENTRY2 to be cacheable (+TLB1_ENTRY3 in DPM).
Issues        : TLB1_ENTRY2 is Internal SRAM in LSM.
                TLB1_ENTRY2 is SRAM0 and TLB1_ENTRY3 is SRAM1 in DPM.
*******************************************************************************/
static asm void MMUSRAMIsCacheable(void)
{
    nofralloc

#if DCACHE_COPYBACK_MODE
    /* copy-back */    
    lis r3, 0x1002     /* Select TLB entry #, define R/W replacement control */
    mtMAS0 r3

    tlbre                 /* Get TLB entry # information */

    mfspr   r3, MAS2
    lis     r4, 0xFFFF
    ori     r4, r4, 0xFFE7    
    and     r3, r3, r4    /* clear I bit (cacheable) */
                          /* clear W bit (copy-back) */
    msync
    tlbwe
    isync
    
    #if (LOCKSTEP_MODE)
    #else // DPM mode
        /* copy-back */    
        lis r3, 0x1003     /* Select TLB entry #, define R/W replacement control */
        mtMAS0 r3

        tlbre                 /* Get TLB entry # information */

        mfspr   r3, MAS2
        lis     r4, 0xFFFF
        ori     r4, r4, 0xFFE7    
        and     r3, r3, r4    /* clear I bit (cacheable) */
                              /* clear W bit (copy-back) */
        msync
        tlbwe
        isync
    #endif

#else

    /* write-through */    
    lis r3, 0x1002     /* Select TLB entry #, define R/W replacement control */
    mtMAS0 r3

    tlbre                 /* Get TLB entry # information */

    mfspr   r3, MAS2
    lis     r4, 0xFFFF
    ori     r4, r4, 0xFFF7    
    and     r3, r3, r4       /* clear I bit (cacheable) */
    ori	    r3, r3, 0x0010   /* set W bit (write-through) */

    msync
    tlbwe
    isync
    
    
    #if (LOCKSTEP_MODE)
    #else // DPM mode
        /* write-through */    
        lis r3, 0x1003     /* Select TLB entry #, define R/W replacement control */
        mtMAS0 r3

        tlbre                 /* Get TLB entry # information */

        mfspr   r3, MAS2
        lis     r4, 0xFFFF
        ori     r4, r4, 0xFFF7    
        and     r3, r3, r4       /* clear I bit (cacheable) */
        ori	    r3, r3, 0x0010   /* set W bit (write-through) */

        msync
        tlbwe
        isync
    #endif
    
#endif    
    
    blr    
}


/*******************************************************************************
Function Name : ICache_init
Engineer      : b05111
Date          : Oct-19-2012
Parameters    : 
Modifies      : 
Returns       : 
Notes         : Invalidate and Enable
Issues        : 
*******************************************************************************/
static asm void ICache_init(void)
{
    nofralloc
    
/**************************************************************************
# FUNCTION     : cfg_CACHE                                               *
# PURPOSE      : This function initializes the CACHE, using L1CSR1       *
#                register.                                                *
#                SEQUENCE:                                                *
#                - Start Cache Invalidate                                *
#                - Wait for invalidation to complete, check for Abort     *
#                  operation and restart invalidation if detected         *
#                - Enable Cache by setting CE bit                       *
#**************************************************************************/
cfg_CACHE:

    /*--------------------------------------------#
    # Invalidate - Set CINV                      #
    # bit in L1CSR1 Register                     #
    #--------------------------------------------*/
    lis   r5, 0x0000
    ori   r5, r5, 0x0002
    msync
    isync
    mtspr SPR_L1CSR1,r5

    /*-------------------------------------------#
    # Mask out CINV and CABT to see if           #
    # invalidation is complete (i.e. CINV=0,     #
    # CABT=0)                                    #
    #-------------------------------------------*/
label_CINV_check:
    /*-------------------------------------------#
    # Load Registers with Masks:                 #
    # Load CINV mask into R8                     #
    # Load CABT mask into R7                     #
    # Load CABT clear mask into R11              #
    #-------------------------------------------*/
    lis   r8, 0x0000
    ori   r8, r8, 0x0002
    lis   r7, 0x0000
    ori   r7, r7, 0x0004
    lis   r11, 0xFFFF
    ori   r11, r11, 0xFFFB
CHECK_CINV:

    /*-------------------------------------------#
    # Read SPR_L1CSR1 register, store in r9      #
    #-------------------------------------------*/
    mfspr r9, SPR_L1CSR1
    /*-------------------------------------------#
    # check for an ABORT of the cache invalidate #
    # operation                                  #
    #-------------------------------------------*/
    and.  r10, r7, r9
    beq   NO_ABORT
    /*-------------------------------------------#
    # If abort detected, clear CABT bit and      #
    # re-run invalidation                        #
    #-------------------------------------------*/
    and.  r10, r11, r9
    msync
    isync
    mtspr SPR_L1CSR1, r10
    b     cfg_CACHE

NO_ABORT:
    /*-------------------------------------------#
    # Check that invalidation has completed -    #
    # (CINV=0). Branch if invalidation not       #
    # complete.                                  #
    #-------------------------------------------*/
    and.  r10, r8, r9
    bne CHECK_CINV

    /*-------------------------------------------#
    # Enable cache by performing a               #
    # read/modify/write of the CE bit in the     #
    # SPR_L1CSR1 register                        #
    #-------------------------------------------*/
    mfspr r5, SPR_L1CSR1
    oris  r5, r5, 0x0000
    ori   r5, r5, 0x0001    /* Store SPR_L1CSR1 value to R5 (CE=1) */
    msync
    isync
    mtspr SPR_L1CSR1, r5        /* Write R5 to SPR_L1CSR1 register */

    blr
/**************************************************************************
# END OF cfg_CACHE                                                        *
#**************************************************************************/
}



/*******************************************************************************
Function Name : DCache_init
Engineer      : b05111
Date          : Oct-19-2012
Parameters    : 
Modifies      : 
Returns       : 
Notes         : Invalidate and Enable
Issues        : 
*******************************************************************************/
static asm void DCache_init(void)
{
    nofralloc
    
/**************************************************************************
# FUNCTION     : cfg_CACHE                                                *
# PURPOSE      : This function initializes the CACHE, using L1CSR1        *
#                register.                                                *
#                SEQUENCE:                                                *
#                - Start Cache Invalidate                                 *
#                - Wait for invalidation to complete, check for Abort     *
#                  operation and restart invalidation if detected         *
#                - Enable Cache by setting CE bit                         *
#**************************************************************************/
cfg_DCACHE:

    /*--------------------------------------------#
    # Invalidate - Set CINV                      #
    # bit in L1CSR0 Register                     #
    #--------------------------------------------*/
    lis   r5, 0x0000
    ori   r5, r5, 0x0002
    msync
    isync
    mtspr SPR_L1CSR0,r5

    /*-------------------------------------------#
    # Mask out CINV and CABT to see if           #
    # invalidation is complete (i.e. CINV=0,     #
    # CABT=0)                                    #
    #-------------------------------------------*/
label_DCINV_check:
    /*-------------------------------------------#
    # Load Registers with Masks:                 #
    # Load CINV mask into R8                     #
    # Load CABT mask into R7                     #
    # Load CABT clear mask into R11              #
    #-------------------------------------------*/
    lis   r8, 0x0000
    ori   r8, r8, 0x0002
    lis   r7, 0x0000
    ori   r7, r7, 0x0004
    lis   r11, 0xFFFF
    ori   r11, r11, 0xFFFB
CHECK_DCINV:

    /*-------------------------------------------#
    # Read SPR_L1CSR0 register, store in r9      #
    #-------------------------------------------*/
    mfspr r9, SPR_L1CSR0
    /*-------------------------------------------#
    # check for an ABORT of the cache invalidate #
    # operation                                  #
    #-------------------------------------------*/
    and.  r10, r7, r9
    beq   D_NO_ABORT
    /*-------------------------------------------#
    # If abort detected, clear CABT bit and      #
    # re-run invalidation                        #
    #-------------------------------------------*/
    and.  r10, r11, r9
    msync
    isync
    mtspr SPR_L1CSR0, r10
    b     cfg_DCACHE

D_NO_ABORT:
    /*-------------------------------------------#
    # Check that invalidation has completed -    #
    # (CINV=0). Branch if invalidation not       #
    # complete.                                  #
    #-------------------------------------------*/
    and.  r10, r8, r9
    bne CHECK_DCINV

    /*-------------------------------------------#
    # Enable cache by performing a               #
    # read/modify/write of the CE bit in the     #
    # SPR_L1CSR0 register                        #
    #-------------------------------------------*/
    mfspr r5, SPR_L1CSR0
    oris  r5, r5, 0x0000
    ori   r5, r5, 0x0001    /* Store SPR_L1CSR0 value to R5 (CE=1) */
    msync
    isync
    mtspr SPR_L1CSR0, r5        /* Write R5 to SPR_L1CSR0 register */

    blr
/**************************************************************************
# END OF cfg_CACHE                                                        *
#**************************************************************************/
}


/*******************************************************************************
Function Name : Enable_DCache_Copyback
Engineer      : b05111
Date          : Oct-19-2012
Parameters    : 
Modifies      : 
Returns       : 
Notes         : Invalidate and Enable
Issues        : 
*******************************************************************************/
#if DCACHE_COPYBACK_MODE
static asm void Enable_DCache_Copyback(void)
{
    /*-------------------------------------------#
    # Enable cache by performing a               #
    # read/modify/write of the DCWM bit in the   #
    # SPR_L1CSR0 register                        #
    #-------------------------------------------*/
    mfspr r5, SPR_L1CSR0
    oris  r5, r5, 0x0010
    ori   r5, r5, 0x0000    /* Store SPR_L1CSR0 value to R5 (DCWM=1) */
    msync
    isync
    mtspr SPR_L1CSR0, r5        /* Write R5 to SPR_L1CSR0 register */

    blr
}
#endif

/*******************************************************************************
Function Name : BranchPredictionEnable
Engineer      : b08110
Date          : Feb-22-2010
Parameters    : 
Modifies      : r3, bucsr
Returns       : 
Notes         : turn on branch prediction and invalidate branch target buffer
Issues        : 
*******************************************************************************/
static asm void BranchPredictionEnable(void)
{
    nofralloc
    
    /* turn on branch prediction and invalidate branch target buffer */
    lis   r3, 0x0
    ori   r3, r3, 0x0201
    mtbucsr r3

    blr   
}


/*******************************************************************************
* Global functions
*******************************************************************************/

/*******************************************************************************
Function Name : Optimizations
Engineer      : b05111
Date          : Nov-14-2014
Parameters    : 
Modifies      : 
Returns       : 
Notes         : 
Issues        : 
*******************************************************************************/
void Optimizations(void)
{
    ICache_init();
    DCache_init();
    #if DCACHE_COPYBACK_MODE
        Enable_DCache_Copyback();
    #endif
    MMUFlashIsCacheable();
    BranchPredictionEnable();
    MMUSRAMIsCacheable();
}

