.section .wakeupo,ax
b sleep_recovery_RAM

.globl cfg_MMU, sleep_recovery, sleep_recovery_RAM

.section .wakeupc,ax
#************************************************************************/
# FUNCTION     : sleep_recovery_RAM                                     */
# PURPOSE      : This function is intended to be used when recovering	*/
#	       : from sleep to a RAM ISR so that an initial TLB3 entry	*/
#	       : is made to maintain accesses to RAM before configuring	*/
#	       : the MMU as traditionally done.  Coming out of sleep to	*/
#	       : RAM only an initial 4KB aligned area is set up in TLB0 */
# INPUT NOTES  : None							*/
# RETURN NOTES : None                                                   */
# WARNING      : Registers used: R5			                */
#************************************************************************/
sleep_recovery_RAM:
# Change the TLB3 to full Internal SRAM.    
    lis   r5, 0x1003	     # Get MAS0 value 0x10030000
    ori   r5,r5, 0x0000 
    mtspr mas0,r5            # mtspr MAS0,r5 
    lis   r5, 0xC000         # Get MAS1 value 0xC0000400
    ori r5,r5, 0x0500
    mtspr mas1,r5            # mtspr MAS1,r5 
    lis   r5, 0x4000         # Get MAS2 value 0x40000008
    ori r5,r5, 0x0008
    mtspr mas2,r5            # mtspr MAS2,r5 
    lis   r5, 0x4000         # Get MAS3 value 0x4000003F
    ori r5,r5, 0x003F
    mtspr mas3,r5            # mtspr MAS3,r5 
    tlbwe                    # Write the entry to the TLB 

    bl cfg_MMU
    bl disable_SWT
	
#####################################################################
# Restore GPRs and SPRs
#####################################################################
loop:
#	b loop
	lis	r4,0xFFFE
	ori	r4,r4,0xC058	#-- CRP.RECPRTR address
	lwz	r1,0x0(r4)	#-- Restore stack pointer from CRP
	lmw	r25,0x78(r1)	#-- Load SPR values back into GPRs
	mtSRR1	r25
	mtSRR0	r26
	mtLR	r27
	mtmsr	r28
	mtCR	r29
	mtXER	r30
	mtCTR	r31		#-- Load SPRs with GPR values
	lmw	r2, 0(r1)	#-- Restore GPRs from stack
	addi	r1,r1,0x94	#-- Deallocate space on stack
	blr			#-- Branch back to calling function

#end of sleep_recovery_RAM


    
#*****************************************************************************/
# FUNCTION     : cfg_MMU                                                     */
# PURPOSE      : This function modifies the MMU TLB (translation lookaside   */
#                 buffer) table by writing to the appropriate MAS registers. */
# INPUT NOTES  : Requires SPRs defined and a data table for the TLB entries  */
#                mmu_tlb0 through mmu_tlb11, mmu_tlb15 from                  */
#                mpc5500_usrdefs.inc.                                        */
# RETURN NOTES : None                                                        */
# WARNING      : Registers used: R3,R5. Commands "msync" and "isync" are not */
#                required around the tlbwe since we are at configuration and */
#                 other background operations cannot be active.              */
#*****************************************************************************/
cfg_MMU:

#***************************************************/
#     setup MMU                                    */
#***************************************************/

#TLB0 =  Pbridge A - No BAM config this
   lis r3, 0x1000
   mtmas0 r3
   lis  r3, 0xC000
   ori r3, r3, 0x0500
   mtmas1	r3

   lis  r3, 0xFFF0
   ori r3, r3, 0x002A
   mtmas2	 r3

   lis  r3, 0xFFF0
   ori  r3, r3, 0x003F
   mtmas3	 r3
   tlbwe

#TLB1 =  1M Internal flash - z6 - No VLE
   lis r3, 0x1001
   mtmas0 r3
   lis  r3, 0xC000
   ori r3, r3, 0x0500
   mtmas1	r3

   lis  r3, 0x0000
   ori r3, r3, 0x0000
   mtmas2	 r3

   lis  r3, 0x0000
   ori  r3, r3, 0x003F
   mtmas3	 r3
   tlbwe

#TLB2 =  1M Internal flash - z0
   lis r3, 0x1002
   mtmas0 r3
   lis  r3, 0xC000
   ori   r3, r3, 0x0500
   mtmas1 r3
   lis  r3, 0x0010
   ori  r3, r3, 0x0020
   mtmas2 r3
   lis  r3, 0x0010
   ori  r3, r3, 0x003F
   mtmas3 r3
   tlbwe

#TLB4 =  pbridgeA
   lis r3, 0x1004
   mtmas0 r3
   lis  r3, 0xC000
   ori  r3, r3, 0x0800
   mtmas1 r3
   lis  r3, 0xC000
   ori  r3, r3, 0x000A
   mtmas2 r3
   lis  r3, 0xC000
   ori  r3, r3, 0x003F
   mtmas3 r3
   tlbwe

   blr
# End of cfg_MMU 
#****************************

#************************************************************************/
# FUNCTION     : disable_SWT                                            */
# PURPOSE      : This function will disable the SWT.			*/
# INPUT NOTES  : None							*/
# RETURN NOTES : None                                                   */
# WARNING      : Registers used: R5,R6		                        */
#************************************************************************/

disable_SWT:
	lis r5,0xFFF3
	ori r5,r5,0x8000				# r5 = address of SWTCR 
	li r6, 0x0
	stw r6,0x0(r5)					# disable SWT 
	blr
# end of of disable_SWT
