// Copyright (c) 2010 Nokia Corporation and/or its subsidiary(-ies).
// All rights reserved.
// This component and the accompanying materials are made available
// under the terms of the License "Eclipse Public License v1.0"
// which accompanies this distribution, and is available
// at the URL " http://www.eclipse.org/legal/epl-v10.html ".
//
// Initial Contributors:
// Nokia Corporation - initial contribution.
//
// Contributors:
//
// Description:
// os\kernelhwsrv\kernel\eka\drivers\power\smpidlehelper.cpp
// Impelentation of helper classes required to implement CPU idle
// functionality in a SMP BSP.

/**
 @file
 @prototype
*/



#ifdef __SMP__


#include <smppower/idlehelper.h>


#ifndef DISABLE_TRACE

extern "C" void btrace_printfn(const TAny *aPtr,const TInt regNum)
    {
    PMBTRACE4(KPrintReg,regNum,aPtr);
    }

extern "C" void btrace_printfnId(const TAny *aPtr,const TInt regNum, TUint aId)
    {
    PMBTRACE8(KPrintReg,0xffu,regNum,aPtr);
    }

#define PRINTREG(Rn)                 \
        asm("stmfd sp!,{r0-r12,lr}"); \
        asm("mov r0,r"#Rn); \
        asm("mov r1,#"#Rn); \
        asm("bl btrace_printfn"); \
        asm("ldmfd sp!,{r0-r12,lr}"); 

#define PRINTREGID(Rn,n)               \
        asm("stmfd sp!,{r0-r12,lr}"); \
        asm("mov r0,r"#Rn); \
        asm("mov r1,#"#Rn); \
        asm("mov r2,#"#n); \
        asm("bl btrace_printfnId"); \
        asm("ldmfd sp!,{r0-r12,lr}"); 


#else
#define PRINTREG(Rn)
#define PRINTREGID(Rn,n)
#endif


/**
   Atomically does the following:
     sets the current cpu idle mask bit to indicate current core wants to idle
     if all enaged cores have set their bit the flag KGlobalIdleFlag is also 
     orred into the idle mask to indicate all cores are going down. In this case
     the function returned true. False otherwise

   aCMask- Bit mask with only current CPU bit set
   Normal Usage:use in idle handler before waiting for all cores down IPI
   
   @pre 
 */		
	
__NAKED__ TBool TIdleSupport::SetLocalAndCheckSetGlobalIdle(TUint32 /*aCpuMask*/)
    {
	asm("ldr    r1,__iAllEngagedCpusMask");                           //r1 = address of iAllEngagedCpusMask
	asm("ldr	r2, [r1]");                                           //r2 = iAllEngagedCpusMask
    asm("add    r1,r1,#4");                                           //r1 = address of iIdlingCpus 
    __DATA_MEMORY_BARRIER_Z__(r12);   
    asm("1: ");
	LDREX(3,1);                                                       // r3 = iIdlingCpus
    asm("orr    r3,r0,r3");                                           // orr in mask for this CPU
    asm("cmp    r3,r2");                                              // compare to iAllEngagedCpusMask
    asm("orreq  r3,r3,#%a0" : : "i" ((TInt)TIdleSupport::KGlobalIdleFlag)); // if equal orr in KGlobalIdleFlag
    STREX(12,3,1);
    asm("cmp    r12, #0 ");                                              // 
	asm("bne    1b ");                                                   // write didn't succeed try again
    __DATA_MEMORY_BARRIER__(r12);
    asm("and    r0,r3,#%a0" : : "i" ((TInt)TIdleSupport::KGlobalIdleFlag));
	__JUMP(,lr);
    asm("__iAllEngagedCpusMask:");
    asm(".word %a0" : : "i" ((TInt)&TIdleSupport::iAllEngagedCpusMask));//
    }

/** 

Wait for all CPUs to reach the sync point. A CPU will only exit this function when all other CPUs
have reached it. 

Works like this:

cpuMask = 1 << NKern::CurrentCpu()
BEGIN_ATOMIC
    stage = iStageAndCPUWaitingMask >> 16
    waitingCpus = iStageAndCPUWaitingMask&iAllCpusMask
    oldstage = stage;
    if (waitingCpus == iAllCpusMask) // we synched already and this is new 
             waitingCpus = 0;
     waitingCpus |= cpuMask
     if (waitingCpus == iAllCpusMask) stage++
     iStageAndCPUWaitingMask = (stage << 16) | waitingCpus
END_ATOMIC
FOREVER 
   if (oldstage!=stage) return
   stage = iStageAndCPUWaitingMask >> 16 // reread stage
END_FOREVER   

*/
__NAKED__ void TSyncPoint::DoSW(TUint32 /*aCpuMask*/)
    {
    asm("stmfd sp!, {r4-r5,lr} ");	
    asm("add r0,r0,#%a0" : : "i"  _FOFF(TSyncPointBase, iStageAndCPUWaitingMask)); // skip vt
    asm("ldr r4,[r0,#4]"); 
    asm("ldr r4,[r4]");
   __DATA_MEMORY_BARRIER_Z__(r12);          // 
    asm("1: ");
	LDREX(2,0);                             // r2 =  iStageAndCPUWaitingMask, r4 = iAllEnagedCpusMask
    asm("mov r5,r2,lsr #16");               // r5 has old staging value
    asm("and r2,r2,r4");                    // r2 has currently waiting cpus
    asm("cmp r2,r4");                       // if r2 == r4 then we previously have had all cpus synched 
    asm("moveq r2,#0");                     // reset 
    asm("orr r2, r2, r1");                  // orr mask for this CPU 
    asm("mov r3,r5");                       // r3 will have new stage
    asm("cmp r2,r4");                       // if r2 == r4 then all cpus have set
    asm("addeq r3,r3,#1");                  // increment new stage count
    asm("orr r2,r2,r3, lsl #16");          
	STREX(12,2,0);                           // try to atomically iStageAndCPUWaitingMask
	asm("cmp r12, #0 ");
	asm("bne 1b ");                         // write didn't succeed try again
    __DATA_MEMORY_BARRIER__(r12);          // ensure that's written
#ifdef SYNCPOINT_WFE	
	asm("ands r2,r2,r4");
    asm("cmp r2,r4");                       // if r2 == r4 then all cpus have set      			
	ARM_SEVcc(CC_EQ);
#endif		
    asm("2: ");
    asm("cmp r3,r5");                       // all (old stage does not equal new stage)
    asm("ldmnefd sp!, {r4-r5,pc}");         // yup return
#ifdef SYNCPOINT_WFE		
	__DATA_MEMORY_BARRIER__(r12);        
	ARM_WFE;
#endif	
    asm("ldr r2,[r0]");                     // otherwise re read iWaitingCpusMask into r5
    __DATA_MEMORY_BARRIER__(r12);           // ensure read is observed
    asm("mov r3,r2,lsr #16");               // re-read new stage
    asm("b 2b");                            // loop back
    }

/** 

Wait for all CPUs to reach the sync point. A CPU will only exit this function when all other CPUs
have reached it or if another CPU has called the Break function. An attempt to wait on a broken
syncpoint will return immediately.

Works like this:

cpuMask = 1 << NKern::CurrentCpu()
BEGIN_ATOMIC
     waitingCpus = iStageAndCPUWaitingMask&iAllEnagedCpusMask
     if (iStageAndCPUWaitingMask & 0x80000000) // sync point is broken
        END_ATOMIC and return

     waitingCpus |= cpuMask
     if (waitingCpus == iAllEnagedCpusMask) waitingCpus |= 0x80000000
     iStageAndCPUWaitingMask = waitingCpus
END_ATOMIC
FOREVER 
     if (iStageAndCPUWaitingMask&0x80000000) break
END_FOREVER   

*/
__NAKED__ void TBreakableSyncPoint::DoSW(TUint32 /*aCpuMask*/)
    {
    asm("stmfd sp!, {r4,lr} ");	
    asm("add r0,r0,#%a0" : : "i"  _FOFF(TSyncPointBase, iStageAndCPUWaitingMask)); // skip vt
    asm("ldr r4,[r0,#4]");
    asm("ldr r4,[r4]");
    __DATA_MEMORY_BARRIER_Z__(r12);          // 
    asm("1: ");
	LDREX(2,0);                             // r2 =  iStageAndCPUWaitingMask, r4 = iAllEnagedCpusMask
    asm("ands r3,r2,#0x80000000");          // 
    asm("bne 3f");                          // sync point broken so return
    asm("and r2,r2,r4");                    // r2 has currently waiting cpus
    asm("orr r2, r2, r1");                  // orr mask for this CPU 
    asm("cmp r2,r4");                       // if r2 == r4 then all cpus have set
    asm("orreq r2,r2,#0x80000000");         // set MSB
	STREX(12,2,0);                          // try to atomically iStageAndCPUWaitingMask
	asm("cmp r12, #0 ");
	asm("bne 1b ");                         // write didn't succeed try again		
    __DATA_MEMORY_BARRIER__(r12);           // ensure that's written
#ifdef SYNCPOINT_WFE	
	asm("ands r3,r2,#0x80000000");          // MSB set?
	ARM_SEVcc(CC_NE);
#endif		
    asm("2: ");
    asm("ands r3,r2,#0x80000000");          // MSB set?	
    asm("ldmnefd sp!, {r4,pc}");            // yup return
#ifdef SYNCPOINT_WFE		
	__DATA_MEMORY_BARRIER__(r12);
	ARM_WFE;
#endif	
    asm("ldr r2,[r0]");                     // otherwise re read iWaitingCpusMask into r5
    //__DATA_MEMORY_BARRIER_Z__(r12);        // ensure read is observed
    asm("b 2b");                            // loop back
    asm("3:");
    CLREX;
#ifdef SYNCPOINT_WFE	 
    __DATA_MEMORY_BARRIER__(r12);           // ensure that's written
	ARM_SEV;
#endif	
    asm("ldmfd sp!, {r4,pc}");            // yup return
    }
	
	
#ifdef PROPER_WFI
__NAKED__ void TIdleSupport::DoWFI()
    {
    __DATA_SYNC_BARRIER_Z__(r12);  // generally good idea to a barrier before WFI            
    ARM_WFI;
	__JUMP(,lr);
    }
#else
void TIdleSupport::DoWFI()
    {
    TInt c=NKern::CurrentCpu();
    FOREVER
        {
        TInt isr = Pending();
        if (isr!=1023) 
            {
            BTRACE0(KIsrPendingCat,isr&0xff);
            break;
            }
        }
    }
#endif

__NAKED__ void TIdleSupport::DoIdleIPI(TUint32 /*aMask*/) 
    {
	//r0 = cpu mask
    asm("ldr    r2,__EnagedCpusMask");  // only IPI enaged cores r2 has enaged core mask addr
    asm("ldr    r2,[r2]");   
    asm("and    r0,r0,r2");            // and out retired cores
    asm("ldr	r1,__KGICAddr");//r1 = address off iGlobalIntDistAddress
    asm("ldr	r1, [r1]");//r1 = address of Hw GIC interrupt dispatcher base			
    __DATA_SYNC_BARRIER_Z__(r12);			// need DSB before sending any IPI
    asm("movs	r0, r0, lsl #16 ");		// CPU mask into bits 16-23 - any bits set in aMask?
    asm("orrne	r0, r0, #%a0" : : "i" ((TInt) IDLE_WAKEUP_IPI_VECTOR));
    asm("strne	r0, [r1, #%a0]" : : "i" _FOFF(GicDistributor, iSoftIrq));	// trigger IPIs if any
    __JUMP(,lr);
    asm("__KGICAddr:");
    asm(".word %a0" : : "i" ((TInt)&TIdleSupport::iGlobalIntDistAddress));
    asm("__EnagedCpusMask:");
    asm(".word %a0" : : "i" ((TInt)&TIdleSupport::iAllEngagedCpusMask));
    }

#ifdef _DEBUG
__NAKED__  TInt TIdleSupport::DoClearIdleIPI()
#else
__NAKED__  void TIdleSupport::ClearIdleIPI()
#endif
    {
    __DATA_SYNC_BARRIER_Z__(r12);			// DSB    
    asm("ldr    r1,__KCPUIFAddr");//r1 = address of iBaseIntIfAddress
    asm("ldr	r1, [r1]");//r1 = address of Hw GIC CPU interrupt interface base address
    asm("ldr    r0,[r1, #%a0]" : : "i" _FOFF(GicCpuIfc, iAck));
    // asm("mov    r0,#%a0" : : "i" ((TInt) IDLE_WAKEUP_IPI_VECTOR)); // has to be!
    asm("str    r0, [r1, #%a0]" : : "i" _FOFF(GicCpuIfc, iEoi));
	__JUMP(,lr);
    asm("__KCPUIFAddr:");
    asm(".word %a0" : : "i" ((TInt)&TIdleSupport::iBaseIntIfAddress));// CPU interrupt interface base address
    }

#ifndef _DEBUG
TInt TIdleSupport::DoClearIdleIPI()
    {
    return 0;
    }
#endif

__NAKED__  TInt TIdleSupport::IntPending()  
    {
	asm("ldr    r1,__KCPUIFAddr");//r1 = address of iBaseIntIfAddress
	asm("ldr	r1, [r1]");//r1 = address of Hw GIC CPU interrupt interface base address
	asm("ldr    r0, [r1, #%a0]" : : "i" _FOFF(GicCpuIfc, iHighestPending));    
	__JUMP(,lr);
    }	


#endif // ifdef __SMP__
