// Copyright (c) 2007-2009 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:

// e32\nkernsmp\arm\ncutilf.cia

// 

//

#include <e32cia.h>

#include <arm.h>

#include <arm_gic.h>

#include <arm_tmr.h>

extern "C" {

extern SVariantInterfaceBlock* VIB;

}

__NAKED__ void Arm::GetUserSpAndLr(TAny*) 

	{

	asm("stmia	r0, {r13, r14}^ ");

	asm("mov	r0, r0"); // NOP needed between stm^ and banked register access

	__JUMP(,	lr);

	}

__NAKED__ void Arm::SetUserSpAndLr(TAny*) 

	{

	asm("ldmia	r0, {r13, r14}^ ");

	asm("mov	r0, r0"); // NOP needed between ldm^ and banked register access

	__JUMP(,	lr);

	}

__NAKED__ TUint32 Arm::Dacr()

	{

	asm("mrc	p15, 0, r0, c3, c0, 0 ");

	__JUMP(,	lr);

	}

__NAKED__ void Arm::SetDacr(TUint32)

	{

	asm("mcr	p15, 0, r0, c3, c0, 0 ");

	__INST_SYNC_BARRIER_Z__(r1);

	__JUMP(,	lr);

	}

__NAKED__ TUint32 Arm::ModifyDacr(TUint32, TUint32)

	{

	asm("mrc	p15, 0, r2, c3, c0, 0 ");

	asm("bic	r2, r2, r0 ");

	asm("orr	r2, r2, r1 ");

	asm("mcr	p15, 0, r2, c3, c0, 0 ");

	__INST_SYNC_BARRIER_Z__(r3);

	asm("mov	r0, r2 ");

	__JUMP(,	lr);

	}

__NAKED__ void Arm::SetCar(TUint32)

	{

	SET_CAR(,	r0);

	__JUMP(,	lr);

	}

/** Get the CPU's coprocessor access register value

@return The value of the CAR, 0 if CPU doesn't have CAR

@publishedPartner

@released

 */

EXPORT_C __NAKED__ TUint32 Arm::Car()

	{

	GET_CAR(,	r0);

	__JUMP(,	lr);

	}

/** Modify the CPU's coprocessor access register value

	Does nothing if CPU does not have CAR.

@param	aClearMask	Mask of bits to clear	(1 = clear this bit)

@param	aSetMask	Mask of bits to set		(1 = set this bit)

@return The original value of the CAR, 0 if CPU doesn't have CAR

@publishedPartner

@released

 */

EXPORT_C __NAKED__ TUint32 Arm::ModifyCar(TUint32 /*aClearMask*/, TUint32 /*aSetMask*/)

	{

	GET_CAR(,	r2);

	asm("bic	r0, r2, r0 ");

	asm("orr	r0, r0, r1 ");

	SET_CAR(,	r0);

	asm("mov	r0, r2 ");

	__JUMP(,	lr);

	}

#ifdef __CPU_HAS_VFP

__NAKED__ void Arm::SetFpExc(TUint32)

	{

#if defined(__CPU_ARM1136__) && !defined(__CPU_ARM1136_ERRATUM_351912_FIXED)

// If we are about to enable VFP, disable dynamic branch prediction

// If we are about to disable VFP, enable dynamic branch prediction if return stack prediction is enabled

	asm("mrs	r3, cpsr ");

	__ASM_CLI();

	asm("mrc	p15, 0, r1, c1, c0, 1 ");

	asm("tst	r0, #%a0" : : "i" ((TInt)VFP_FPEXC_EN) );

	asm("bic	r1, r1, #2 ");				// clear DB bit (disable dynamic prediction)

	asm("and	r2, r1, #1 ");				// r2 bit 0 = RS bit (1 if return stack enabled)

	asm("orreq	r1, r1, r2, lsl #1 ");		// if VFP is being disabled set DB = RS

	asm("mcr	p15, 0, r1, c1, c0, 1 ");

	asm("mcr	p15, 0, r2, c7, c5, 6 ");	// flush BTAC

	VFP_FMXR(,	VFP_XREG_FPEXC,0);

	__INST_SYNC_BARRIER_Z__(r12);

	asm("msr	cpsr, r3 ");

	__JUMP(,	lr);

#else

	VFP_FMXR(,	VFP_XREG_FPEXC,0);

	__JUMP(,	lr);

#endif

	}

#endif

/** Get the value of the VFP FPEXC register

@return The value of FPEXC, 0 if there is no VFP

@publishedPartner

@released

 */

EXPORT_C __NAKED__ TUint32 Arm::FpExc()

	{

#ifdef __CPU_HAS_VFP

	VFP_FMRX(,	0,VFP_XREG_FPEXC);

#else

	asm("mov	r0, #0 ");

#endif

	__JUMP(,	lr);

	}

/** Modify the VFP FPEXC register

	Does nothing if there is no VFP

@param	aClearMask	Mask of bits to clear	(1 = clear this bit)

@param	aSetMask	Mask of bits to set		(1 = set this bit)

@return The original value of FPEXC, 0 if no VFP present

@publishedPartner

@released

 */

EXPORT_C __NAKED__ TUint32 Arm::ModifyFpExc(TUint32 /*aClearMask*/, TUint32 /*aSetMask*/)

	{

#ifdef __CPU_HAS_VFP

	VFP_FMRX(,	12,VFP_XREG_FPEXC);

	asm("bic	r0, r12, r0 ");

	asm("orr	r0, r0, r1 ");

#if defined(__CPU_ARM1136__) && !defined(__CPU_ARM1136_ERRATUM_351912_FIXED)

// If we are about to enable VFP, disable dynamic branch prediction

// If we are about to disable VFP, enable dynamic branch prediction if return stack prediction is enabled

	asm("mrs	r3, cpsr ");

	__ASM_CLI();

	asm("mrc	p15, 0, r1, c1, c0, 1 ");

	asm("tst	r0, #%a0" : : "i" ((TInt)VFP_FPEXC_EN) );

	asm("bic	r1, r1, #2 ");				// clear DB bit (disable dynamic prediction)

	asm("and	r2, r1, #1 ");				// r2 bit 0 = RS bit (1 if return stack enabled)

	asm("orreq	r1, r1, r2, lsl #1 ");		// if VFP is being disabled set DB = RS

	asm("mcr	p15, 0, r1, c1, c0, 1 ");

	asm("mcr	p15, 0, r2, c7, c5, 6 ");	// flush BTAC

	VFP_FMXR(,	VFP_XREG_FPEXC,0);

	__INST_SYNC_BARRIER_Z__(r12);

	asm("msr	cpsr, r3 ");

#else

	VFP_FMXR(,	VFP_XREG_FPEXC,0);

#endif	// erratum 351912

	asm("mov	r0, r12 ");

#else	// no vfp

	asm("mov	r0, #0 ");

#endif

	__JUMP(,	lr);

	}

/** Get the value of the VFP FPSCR register

@return The value of FPSCR, 0 if there is no VFP

@publishedPartner

@released

 */

EXPORT_C __NAKED__ TUint32 Arm::FpScr()

	{

#ifdef __CPU_HAS_VFP

	VFP_FMRX(,	0,VFP_XREG_FPSCR);

#else

	asm("mov	r0, #0 ");

#endif

	__JUMP(,	lr);

	}

/** Modify the VFP FPSCR register

	Does nothing if there is no VFP

@param	aClearMask	Mask of bits to clear	(1 = clear this bit)

@param	aSetMask	Mask of bits to set		(1 = set this bit)

@return The original value of FPSCR, 0 if no VFP present

@publishedPartner

@released

 */

EXPORT_C __NAKED__ TUint32 Arm::ModifyFpScr(TUint32 /*aClearMask*/, TUint32 /*aSetMask*/)

	{

#ifdef __CPU_HAS_VFP

	VFP_FMRX(,	2,VFP_XREG_FPSCR);

	asm("bic	r0, r2, r0 ");

	asm("orr	r0, r0, r1 ");

	VFP_FMXR(,	VFP_XREG_FPSCR,0);

	asm("mov	r0, r2 ");

#else

	asm("mov	r0, #0 ");

#endif

	__JUMP(,	lr);

	}

/** Detect whether NEON is present

@return ETrue if present, EFalse if not

@internalTechnology

@released

 */

#if defined(__CPU_HAS_VFP) && defined(__VFP_V3)

__NAKED__ TBool Arm::NeonPresent()

	{

	asm("mov	r0, #0 ");										// Not present

	VFP_FMRX(,	1,VFP_XREG_FPEXC);								// Save VFP state

	asm("orr	r2, r1, #%a0" : : "i" ((TInt)VFP_FPEXC_EN));

	VFP_FMXR(,	VFP_XREG_FPEXC,1);								// Enable VFP

	VFP_FMRX(,	2,VFP_XREG_MVFR0);								// Read MVFR0

	asm("tst	r2, #%a0" : : "i" ((TInt)VFP_MVFR0_ASIMD32));	// Check to see if all 32 Advanced SIMD registers are present

	asm("beq	0f ");											// Skip ahead if not

	GET_CAR(,	r2);

	asm("tst	r2, #%a0" : : "i" ((TInt)VFP_CPACR_ASEDIS));	// Check to see if ASIMD is disabled

	asm("bne	0f ");											// Skip ahead if so

	asm("tst	r2, #%a0" : : "i" ((TInt)VFP_CPACR_D32DIS));	// Check to see if the upper 16 registers are disabled

	asm("moveq	r0, #1" );										// If not then eport NEON present

	asm("0: ");

	VFP_FMXR(,VFP_XREG_FPEXC,1);								// Restore VFP state

	__JUMP(,	lr);

	}

#endif

#ifdef __CPU_HAS_MMU

__NAKED__ TBool Arm::MmuActive()

	{

	asm("mrc	p15, 0, r0, c1, c0, 0 ");

	asm("and	r0, r0, #1 ");

	__JUMP(,	lr);

	}

// Returns the content of Translate Table Base Register 0.

// To get physical address of the level 1 table, on some platforms this must be orred with 0xffff8000 (to get rid of table walk cache attributes)

__NAKED__ TUint32 Arm::MmuTTBR0()

	{

	asm("mrc	p15, 0, r0, c2, c0, 0 ");

	__JUMP(,	lr);

	}

#endif

#if defined(__NKERN_TIMESTAMP_USE_LOCAL_TIMER__)

#error Use of local timer for NKern::Timestamp() no longer supported

#elif defined(__NKERN_TIMESTAMP_USE_SCU_GLOBAL_TIMER__)

// Code to access global timer in Cortex A9 r1p0

#ifndef	__CPU_ARM_HAS_GLOBAL_TIMER_BLOCK

#error NKern::Timestamp() wants global timer, but global timer not present.

#endif

EXPORT_C __NAKED__ TUint64 NKern::Timestamp()

	{

	asm("ldr	r3, __TheScheduler ");

	asm("mrs	r12, cpsr ");				// r12 = saved interrupt mask

	asm("stmfd	sp!, {r4-r7} ");

	asm("ldr	r5, [r3, #%a0]" : : "i" _FOFF(TScheduler,iSub[0]));						// r5->subscheduler for CPU0

	asm("ldr	r4, [r3, #%a0]" : : "i" _FOFF(TScheduler,iSX.iGlobalTimerAddr));		// r4 points to global timer

	__ASM_CLI();							// disable all interrupts

	asm("ldr	r6, [r5, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iTicksSinceLastSync));	// r6 = count value of last frequency change (low)

	asm("ldr	r7, [r5, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iLastTimerSet));		// r7 = count value of last frequency change (high)

	asm("ldr	r2, [r4, #%a0]" : : "i" _FOFF(ArmGlobalTimer,iTimerCountHigh));			// r2 = current timer counter high word

	// To read 64 bit timer value, read high, low, high

	// If two high values match -> OK, else repeat

	asm("1:		");

	asm("mov	r1, r2 ");					// r1 = previous value of timer counter high word

	asm("ldr	r0, [r4, #%a0]" : : "i" _FOFF(ArmGlobalTimer,iTimerCountLow));			// r0 = current timer counter low word

	asm("ldr	r2, [r4, #%a0]" : : "i" _FOFF(ArmGlobalTimer,iTimerCountHigh));			// r2 = current timer counter high word

	asm("cmp	r1, r2 ");					// high word changed?

	asm("bne	1b ");						// if so, retry

	// Now have R1:R0 = 64 bit global timer count

	asm("ldr	r3, [r5, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iNTimerPeriodM));		// r3 = period multiplier

	asm("ldr	r4, [r5, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iNTimerPeriodS));		// r4 = period multiplier shift

	asm("subs	r6, r0, r6 ");				// r7:r6 = ticks from last frequency change

	asm("sbcs	r7, r1, r7 ");

	asm("umull	r0, r1, r6, r3 ");

	asm("mov	r2, #0 ");

	asm("umlal	r1, r2, r7, r3 ");			// r2:r1:r0 = delta * period multiplier

	asm("rsb	r3, r4, #32 ");

	asm("ldr	r6, [r5, #%a0]!" : : "i" _FOFF(TSubScheduler,iSSX.iLastSyncTime));		// r6 = timestamp at last freq change (low)

	asm("ldr	r7, [r5, #4] ");														// r7 = timestamp at last freq change (high)

	asm("msr	cpsr, r12 ");				// restore interrupts

	asm("movs	r0, r0, lsr r4 ");			// rounding bit into C

	asm("orr	r0, r0, r1, lsl r3 ");

	asm("mov	r1, r1, lsr r4 ");

	asm("orr	r1, r1, r2, lsl r3 ");		// r1:r0 = (delta * period multiplier) >> period multiplier shift

	asm("adcs	r0, r0, r6 ");				// scaled delta + timestamp at last freq change

	asm("adcs	r1, r1, r7 ");

	asm("ldmfd	sp!, {r4-r7} ");

	__JUMP(,lr);

	asm("__TheScheduler: ");

	asm(".word	%a0" : : "i" ((TInt)&TheScheduler));

	}

#elif defined(__NKERN_TIMESTAMP_USE_INLINE_BSP_CODE__)

#define __DEFINE_NKERN_TIMESTAMP_ASM__

#include <variant_timestamp.h>

#undef __DEFINE_NKERN_TIMESTAMP_ASM__

#elif defined(__NKERN_TIMESTAMP_USE_BSP_CALLOUT__)

// Code to call function defined in variant

#else

#error No definition for NKern::Timestamp()

#endif

extern "C" __NAKED__ TLinAddr get_sp_svc()

	{

	asm("mrs	r1, cpsr ");

	__ASM_CLI_MODE(MODE_SVC);

	asm("mov	r0, sp ");

	asm("msr	cpsr, r1 ");

	__JUMP(,	lr);

	}

extern "C" __NAKED__ TLinAddr get_lr_svc()

	{

	asm("mrs	r1, cpsr ");

	__ASM_CLI_MODE(MODE_SVC);

	asm("mov	r0, lr ");

	asm("msr	cpsr, r1 ");

	__JUMP(,	lr);

	}

/** Get the return address from an ISR

Call only from an ISR

@internalTechnology

*/

EXPORT_C __NAKED__ TLinAddr Arm::IrqReturnAddress()

	{

	asm("mrs	r1, cpsr ");

	__ASM_CLI();

	asm("and	r0, r1, #0x1f ");

	asm("cmp	r0, #0x11 ");				// mode_fiq ?

	asm("beq	1f ");

	__ASM_CLI_MODE(MODE_SVC);

	asm("ldr	r0, [sp, #%a0]" : : "i" _FOFF(SThreadExcStack,iR15));

	asm("msr	cpsr, r1 ");

	__JUMP(,	lr);

	asm("1:		");

	GET_RWNO_TID(,r3);

	asm("ldr	r2, [r3, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iFiqStackTop));	// if so, r2->top of FIQ stack

	asm("ldr	r0, [r2, #-4] ");			// get return address

	asm("msr	cpsr, r1 ");

	__JUMP(,	lr);

	}

#if defined(__INCLUDE_SPIN_LOCK_CHECKS__)

#define	__ASM_CALL(func)						\

	asm("str	lr, [sp, #-4]! ");				\

	asm("bl "	CSM_CFUNC(func));				\

	asm("ldr	lr, [sp], #4 ");

#define	SPIN_LOCK_ENTRY_CHECK()			__ASM_CALL(spin_lock_entry_check)

#define	SPIN_LOCK_MARK_ACQ()			__ASM_CALL(spin_lock_mark_acq)

#define	SPIN_UNLOCK_ENTRY_CHECK()		__ASM_CALL(spin_unlock_entry_check)

#define	RWSPIN_RLOCK_ENTRY_CHECK()		__ASM_CALL(rwspin_rlock_entry_check)

#define	RWSPIN_RLOCK_MARK_ACQ()			__ASM_CALL(rwspin_rlock_mark_acq)

#define	RWSPIN_RUNLOCK_ENTRY_CHECK()	__ASM_CALL(rwspin_runlock_entry_check)

#define	RWSPIN_WLOCK_ENTRY_CHECK()		__ASM_CALL(rwspin_wlock_entry_check)

#define	RWSPIN_WLOCK_MARK_ACQ()			__ASM_CALL(rwspin_wlock_mark_acq)

#define	RWSPIN_WUNLOCK_ENTRY_CHECK()	__ASM_CALL(rwspin_wunlock_entry_check)

#else

#define	SPIN_LOCK_ENTRY_CHECK()

#define	SPIN_LOCK_MARK_ACQ()

#define	SPIN_UNLOCK_ENTRY_CHECK()

#define	RWSPIN_RLOCK_ENTRY_CHECK()

#define	RWSPIN_RLOCK_MARK_ACQ()

#define	RWSPIN_RUNLOCK_ENTRY_CHECK()

#define	RWSPIN_WLOCK_ENTRY_CHECK()

#define	RWSPIN_WLOCK_MARK_ACQ()

#define	RWSPIN_WUNLOCK_ENTRY_CHECK()

#endif

/******************************************************************************

 * Spin locks

 *

 * [this+0]		in count (byte)

 * [this+1]		out count (byte)

 * [this+6]		order (byte)

 * [this+7]		holding CPU (byte)

 ******************************************************************************/

#if defined(__INCLUDE_SPIN_LOCK_CHECKS__)

extern "C" __NAKED__ void spin_lock_entry_check()

	{

	/* R0 points to lock */

	asm("stmfd sp!, {r1,r2,r3,r12} ");

	asm("mrs r12, cpsr ");

	__ASM_CLI();

	GET_RWNO_TID(, r1);						/* R1->SubScheduler */

	asm("cmp r1, #0 ");

	asm("beq slec_ok ");					/* Skip checks if subscheduler not yet initialised */

	asm("ldrh r2, [r0, #6] ");				/* R2[8:15]=holding CPU, R2[0:7]=order */

	asm("tst r2, #0xE0 ");

	asm("bne slec_preemption ");			/* This lock requires preemption to be disabled */

	/* check interrupts disabled, if interrupts/preemption is not disabled 

	there is a risk of same core deadlock occuring, hence this check and

	run-time assert to ensure code stays safe */

	asm("and r3, r12, #%a0" : : "i" ((TInt)KAllInterruptsMask));

	asm("cmp r3, #%a0" : : "i" ((TInt)KAllInterruptsMask));		/* Check interrupts masked */

	asm("beq slec_1 ");						/* Yes - OK */

	__ASM_CRASH();							/* No - die */

	asm("slec_preemption: ");

	asm("and r3, r2, #0xFF ");

	asm("cmp r3, #0xFF ");					/* check for EOrderNone */

	asm("beq slec_1 ");						/* EOrderNone - don't check interrupts or preemption */

	asm("and r3, r12, #0x1F ");

	asm("cmp r3, #0x13 ");					/* Make sure we're in mode_svc */

	asm("ldr r3, [r1, #%a0]" : : "i" _FOFF(TSubScheduler, iKernLockCount));

	asm("bne slec_preemption_die ");		/* If not, die */

	asm("cmp r3, #0 ");

	asm("bne slec_1 ");						/* Preemption disabled - OK */

	asm("slec_preemption_die: ");

	__ASM_CRASH();							/* Preemption enabled - die */

	asm("slec_1: ");

	asm("ldr r3, [r1, #%a0]" : : "i" _FOFF(TSubScheduler, iCpuNum));

	asm("cmp r3, r2, lsr #8 ");				/* Test if held by current CPU */

	asm("bne slec_2 ");						/* Not already held by this CPU - OK */

	__ASM_CRASH();							/* Already held by this CPU - die */

	asm("slec_2: ");

	asm("ldr r3, [r1, #%a0]!" : : "i" _FOFF(TSubScheduler, iSpinLockOrderCheck));

	asm("ldr r1, [r1, #4] ");				/* r3=low word of iSpinLockOrderCheck, r1=high word */

	asm("cmp r3, #0 ");

	asm("addeq r2, r2, #0x20000000 ");		/* if low word zero, add 32 to LS1 index ... */

	asm("moveq r3, r1 ");					/* ... and r3=high word ... */

	asm("subs r1, r3, #1 ");				/* R1 = R3 with all bits up to and including LS1 flipped */

	asm("beq slec_ok ");					/* If all bits zero, no locks held so OK */

	asm("eor r3, r3, r1 ");					/* Clear all bits above LS1 */

	CLZ(1,3);								/* R1 = 31 - bit number of LS1 */

	asm("rsb r1, r1, #31 ");				/* R1 = bit number of LS1 */

	asm("add r1, r1, r2, lsr #24 ");		/* add 32 if we were looking at high word */

	asm("mov r2, r2, lsl #24 ");			/* this lock's order value into R2 high byte */

	asm("cmp r1, r2, asr #24 ");			/* compare current lowest order lock to sign-extended order value */

	asm("bgt slec_ok ");					/* if this lock's order < current lowest, OK */

	__ASM_CRASH();							/* otherwise die */

	asm("slec_ok: ");

	asm("msr cpsr, r12 ");					/* restore interrupts */

	asm("ldmfd sp!, {r1,r2,r3,r12} ");

	__JUMP(,lr);

	}

extern "C" __NAKED__ void spin_lock_mark_acq()

	{

	/* R0 points to lock */

	asm("stmfd sp!, {r1,r2,r3,r12} ");

	asm("mrs r12, cpsr ");

	__ASM_CLI();

	GET_RWNO_TID(, r1);						/* R1->SubScheduler */

	asm("cmp r1, #0 ");

	asm("beq slma_ok ");					/* Skip checks if subscheduler not yet initialised */

	asm("ldr r3, [r1, #%a0]" : : "i" _FOFF(TSubScheduler, iCpuNum));

	asm("ldrb r2, [r0, #6] ");				/* R2 = lock order value */

	asm("add r1, r1, #%a0" : : "i" _FOFF(TSubScheduler, iSpinLockOrderCheck));

	asm("strb r3, [r0, #7] ");				/* set byte 7 to holding CPU number */