// Copyright (c) 2008-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\ncsched.cia
// 
//

// NThreadBase member data
#define __INCLUDE_NTHREADBASE_DEFINES__

// TDfc member data
#define __INCLUDE_TDFC_DEFINES__

#include <e32cia.h>
#include <arm.h>
#include "nkern.h"
#include <arm_gic.h>
#include <arm_scu.h>
#include <arm_tmr.h>
//#include <highrestimer.h>
//#include "emievents.h"

#ifdef _DEBUG
#define ASM_KILL_LINK(rp,rs)	asm("mov "#rs", #0xdf ");\
								asm("orr "#rs", "#rs", "#rs", lsl #8 ");\
								asm("orr "#rs", "#rs", "#rs", lsl #16 ");\
								asm("str "#rs", ["#rp"] ");\
								asm("str "#rs", ["#rp", #4] ");
#else
#define ASM_KILL_LINK(rp,rs)
#endif

#define ALIGN_STACK_START			\
	asm("mov r12, sp");				\
	asm("tst sp, #4");				\
	asm("subeq sp, sp, #4");		\
	asm("str r12, [sp,#-4]!")

#define ALIGN_STACK_END				\
	asm("ldr sp, [sp]")


//#define __DEBUG_BAD_ADDR

extern "C" void NewThreadTrace(NThread* a);
extern "C" void send_accumulated_resched_ipis();
extern "C" void wake_up_for_ipi(TSubScheduler*, TInt);


__NAKED__ void TScheduler::Reschedule()
	{
	//
	// Enter in mode_svc with kernel locked, interrupts can be on or off
	// Exit in mode_svc with kernel unlocked, interrupts off
	// NOTE: STACK ALIGNMENT UNKNOWN ON ENTRY
	// NOTE: R4-R11 are modified
	//
	asm("mov	r2, sp ");					// bit 0 will be reschedule flag
	asm("bic	sp, sp, #4 ");				// align stack
	GET_RWNO_TID(,r0)						// r0->TSubScheduler
	asm("stmfd	sp!, {r2,lr} ");			// save original SP/resched flag, return address
	__ASM_CLI();							// interrupts off
	asm("ldr	r1, [r0, #%a0]" : : "i" (_FOFF(TSubScheduler,iDfcPendingFlag)&~3));	// check iDfcPendingFlag and iExIDfcPendingFlag
	asm("mov	r11, r0 ");					// r11->TSubScheduler
	asm("ldr	r10, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iLocalTimerAddr));	// r10->CPU local timer

	asm("start_resched: ");
	asm("movs	r1, r1, lsr #16 ");			// check if IDFCs or ExIDFCs pending

	asm("blne "	CSM_ZN13TSubScheduler9QueueDfcsEv);		// queue any pending DFCs
	asm("ldrb	r1, [r11, #%a0]" : : "i" _FOFF(TSubScheduler,iRescheduleNeededFlag));
	asm("ldr	r3, [sp, #0] ");
	asm("mrs	r2, spsr ");				// r2=spsr_svc
	asm("cmp	r1, #0 ");					// check if a reschedule is required
	asm("beq	no_resched_needed ");		// branch out if not
	__ASM_STI();							// interrupts back on
	asm("orr	r3, r3, #1 ");
	asm("str	r3, [sp, #0] ");			// set resched flag
	asm("stmfd	sp!, {r0,r2} ");			// store SPSR_SVC
	asm("ldr	r5, [r11, #%a0]" : : "i" _FOFF(TSubScheduler,iCurrentThread));
#ifdef __CPU_ARMV7
	asm("mrc	p14, 6, r7, c1, c0, 0 ");	// r7 = TEEHBR
#else
	asm("mov	r7, #0 ");
#endif
	GET_RWRO_TID(,r8);						// r8 = User RO Thread ID
	GET_RWRW_TID(,r9);						// r9 = User RW Thread ID
#ifdef __CPU_HAS_VFP
	VFP_FMRX(,0,VFP_XREG_FPEXC);			// r0 = FPEXC
	asm("bic r0, r0, #%a0" : : "i" ((TInt)VFP_FPEXC_EN) ); // Store FPEXC with VFP disabled in case this thread runs on a different core next time
#else
	asm("mov	r0, #0 ");
#endif
	GET_CAR(,	r1);						// r1 = CAR
	asm("mrc	p15, 0, r12, c3, c0, 0 ");	// r12 = DACR
	asm("ldr	r4, [r11, #%a0]" : : "i" _FOFF(TSubScheduler,iInitialThread));

	// Save auxiliary registers
	// R0=FPEXC, R1=CAR, R7=TEEHBR, R8=RWROTID, R9=RWRWTID, R12=DACR
	asm("sub	sp, sp, #%a0" : : "i" _FOFF(SThreadReschedStack,iSpare));
	asm("str	sp, [r5, #%a0]" : : "i" _FOFF(NThread,iSavedSP));	// store original thread's stack pointer
	asm("stmia	sp, {r0-r1,r7-r9,r12} ");

	// We must move to a temporary stack before selecting the next thread.
	// This is because another CPU may begin executing this thread before the
	// select_next_thread() function returns and our stack would then be
	// corrupted. We use the stack belonging to this CPU's initial thread since
	// we are guaranteed that will never run on another CPU.
	asm("ldr	sp, [r4, #%a0]" : : "i" _FOFF(NThread,iSavedSP));

	asm("select_thread: ");
	asm("mov	r0, r11 ");
	asm("bl "	CSM_ZN13TSubScheduler16SelectNextThreadEv );	// also sets r0->iCurrentThread
#ifdef BTRACE_CPU_USAGE
	asm("ldr	r2, __BTraceFilter ");
#endif
	asm("movs	r3, r0 ");					// r3 = new thread (might be 0)
	asm("ldrne	sp, [r0, #%a0]" : : "i" _FOFF(NThread,iSavedSP));	// if a thread has been selected, move to its stack
	asm("beq	no_thread ");				// branch out if no thread is ready

#ifdef BTRACE_CPU_USAGE
	asm("ldrb	r1, [r2, #4] ");			// check category 4 trace
	asm("cmp	r1, #0 ");
	asm("beq	1f ");
	asm("stmfd	sp!, {r0-r3} ");
	asm("bl		NewThreadTrace ");
	asm("ldmfd	sp!, {r0-r3} ");
	asm("1: ");
#endif	// BTRACE_CPU_USAGE

	asm("cmp	r3, r5 ");					// same thread?
	asm("beq	same_thread ");
	asm("ldrb	r1, [r3, #%a0]" : : "i" _FOFF(NThreadBase, i_ThrdAttr));
	asm("ldr	r4, [r11, #%a0]" : : "i" _FOFF(TSubScheduler, iScheduler));
	asm("mov	r2, r3, lsr #6 ");			// r2=current thread>>6
	asm("tst	r1, #%a0" : : "i" ((TInt)KThreadAttAddressSpace));	// address space required?
	asm("ldrne	r4, [r4, #%a0]" : : "i" _FOFF(TScheduler,iProcessHandler));	// if so, get pointer to process handler

	// we are doing a thread switch so restore new thread's auxiliary registers
	// R0=FPEXC, R1=CAR, R7=TEEHBR, R8=RWROTID, R9=RWRWTID, R12=DACR
	asm("ldmia	sp, {r0-r1,r7-r9,r12} ");

#ifdef __CPU_ARMV7
	asm("mcr	p14, 6, r7, c1, c0, 0 ");	// r7 = TEEHBR
#endif
	SET_RWRO_TID(,r8);						// r8 = User RO Thread ID
	SET_RWRW_TID(,r9);						// r9 = User RW Thread ID
#ifdef __CPU_HAS_VFP
	VFP_FMXR(,VFP_XREG_FPEXC,0);			// r0 = FPEXC
#endif
	SET_CAR(,	r1);						// r1 = CAR
	asm("mcr	p15, 0, r12, c3, c0, 0 ");	// r12 = DACR

	asm("beq	no_as_switch ");			// skip if address space change not required

	// Do address space switching
	// Handler called with:
	// r11->subscheduler, r3->current thread
	// r9->new address space, r5->old address space
	// Must preserve r10,r11,r3, can modify other registers
	asm("ldr	r5, [r11, #%a0]" : : "i" _FOFF(TSubScheduler,iAddressSpace));	// get current address space ptr
	asm("ldr	r9, [r3, #%a0]" : : "i" _FOFF(NThreadBase,iAddressSpace));		// get new address space ptr
	asm("adr	lr, as_switch_return ");
	__JUMP(,	r4);

	asm("no_as_switch: ");
	asm("mrc	p15, 0, r4, c13, c0, 1 ");	// r4 = CONTEXTID (threadID:ASID)
	asm("and	r4, r4, #0xff ");			// isolate ASID
	asm("orr	r2, r4, r2, lsl #8 ");		// r2 = new ContextID (new thread ID : ASID)
	__DATA_SYNC_BARRIER_Z__(r12);			// needed before change to ContextID
	asm("mcr	p15, 0, r2, c13, c0, 1 ");	// set ContextID (ASID + debugging thread ID)
	__INST_SYNC_BARRIER__(r12);
#ifdef __CPU_NEEDS_BTAC_FLUSH_AFTER_ASID_CHANGE
	asm("mcr	p15, 0, r12, c7, c5, 6 ");	// flush BTAC
#endif

	asm("as_switch_return: ");
	asm("same_thread: ");
	asm("add	sp, sp, #%a0" : : "i" _FOFF(SThreadReschedStack,iSpare));	// step past auxiliary registers
 	asm("ldmib	sp!, {r2,r12} ");			// r2=SPSR_SVC, r12=original SP + resched flag
	__ASM_CLI();							// interrupts off
	asm("ldr	r1, [r11, #%a0]" : : "i" _FOFF(TSubScheduler,iRescheduleNeededFlag));
	asm("msr	spsr, r2 ");				// restore spsr_svc
	asm("mov	r0, r11 ");
	asm("mov	r2, r12 ");					// r2 = original SP + reschedule flag
	asm("cmp	r1, #0 ");					// check for more IDFCs and/or another reschedule
	asm("bne	start_resched ");			// loop if required
	asm("ldr	r14, [r3, #%a0]" : : "i" _FOFF(NThreadBase,iCsFunction));
	asm("ldr	r12, [r11, #%a0]" : : "i" _FOFF(TSubScheduler,iReschedIPIs));
	asm("str	r1, [r11, #%a0]" : : "i" _FOFF(TSubScheduler,iKernLockCount));
	asm("cmp	r14, #%a0" : : "i" ((TInt)NThreadBase::ECSDivertPending));
	asm("ldr	lr, [sp, #4] ");			// restore R10, R11, return address
	asm("bic	sp, r2, #3 ");				// restore initial unaligned stack pointer
	asm("and	r2, r2, #1 ");				// r2 = reschedule flag
	asm("beq	resched_thread_divert ");

	// Return with:	R0=&SubScheduler, R1=0, R2=TRUE if reschedule occurred, R3=iCurrentThread
	//				R12=iReschedIPIs
	__JUMP(,	lr);

	asm("no_resched_needed: ");
	asm("ldr	r3, [r11, #%a0]" : : "i" _FOFF(TSubScheduler,iCurrentThread));
	asm("ldr	r12, [r11, #%a0]" : : "i" _FOFF(TSubScheduler,iReschedIPIs));
	asm("mov	r0, r11 ");
	asm("str	r1, [r11, #%a0]" : : "i" _FOFF(TSubScheduler,iKernLockCount));
	asm("ldr	r2, [r3, #%a0]" : : "i" _FOFF(NThreadBase,iCsFunction));
	asm("cmp	r2, #%a0" : : "i" ((TInt)NThreadBase::ECSDivertPending));
	asm("ldmfd	sp, {r2,lr} ");				// r2 = original SP + reschedule flag, restore lr
	asm("bic	sp, r2, #3 ");				// restore initial unaligned stack pointer
	asm("and	r2, r2, #1 ");				// r2 = reschedule flag
	asm("beq	resched_thread_divert ");

	// Return with:	R0=&SubScheduler, R1=0, R2=TRUE if reschedule occurred, R3=iCurrentThread
	//				R12=iReschedIPIs
	__JUMP(,	lr);

	asm("resched_thread_divert: ");
	asm("mov	r1, #1 ");
	asm("str	r1, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iKernLockCount));
	asm("bic	sp, sp, #4 ");				// align stack
	asm("stmfd	sp!, {r0-r5,r12,lr} ");		// save registers for diagnostic purposes
	asm("mov	r4, r3 ");					// don't really need to bother about registers since thread is exiting

	// need to send any outstanding reschedule IPIs
	asm("cmp	r12, #0 ");
	asm("blne " CSM_CFUNC(send_accumulated_resched_ipis));

	__ASM_STI();
	asm("ldrb	r1, [r4, #%a0]" : : "i" _FOFF(NThreadBase,iFastMutexDefer));
	asm("cmp	r1, #1 ");
	asm("bne	1f ");
	__ASM_CRASH();
	asm("1: ");
	asm("mov	r2, #0 ");
	asm("strb	r2, [r4, #%a0]" : : "i" _FOFF(NThreadBase,iFastMutexDefer));
	asm("mov	r0, r4 ");
	asm("bl "	CSM_ZN11NThreadBase4ExitEv );
	__ASM_CRASH();	// shouldn't get here

	// There is no thread ready to run
	// R11->TSubScheduler, R1=unknown, R2=0, R3=__BTraceFilter, R12=unknown
	asm("no_thread: ");
	__ASM_CLI();
	asm("ldr	r12, [r11, #%a0]" : : "i" _FOFF(TSubScheduler,iReschedIPIs));
	asm("mov	r0, r11 ");
	asm("cmp	r12, #0 ");
	asm("blne " CSM_CFUNC(send_accumulated_resched_ipis));
	__ASM_STI();
	__DATA_SYNC_BARRIER_Z__(r1);
	ARM_WFI;
	asm("no_thread2: ");
	asm("ldr	r1, [r11, #%a0]" : : "i" (_FOFF(TSubScheduler,iDfcPendingFlag)&~3));	// check iDfcPendingFlag and iExIDfcPendingFlag
	asm("mov	r0, r11 ");
	asm("movs	r1, r1, lsr #16 ");
	asm("beq	no_thread ");
	asm("bl "	CSM_ZN13TSubScheduler9QueueDfcsEv);		// queue any pending DFCs
	asm("ldrb	r1, [r11, #%a0]" : : "i" _FOFF(TSubScheduler,iRescheduleNeededFlag));
	asm("cmp	r1, #0 ");					// check if a reschedule is required
	asm("beq	no_thread2 ");
	asm("b		select_thread ");



/******************************************************************************
 Missed out stuff:
	EMI EVENT LOGGING
	__CPU_ARM1136_ERRATUM_351912_FIXED
	Debug hooks in the scheduler
 ******************************************************************************/

	asm("__BTraceFilter: ");
	asm(".word %a0 " : : "i" ((TInt)&BTraceData.iFilter[0]));
	};


/** 
 * Returns the range of linear memory which inserting the scheduler hooks needs to modify.
 * 
 * @param aStart Set to the lowest memory address which needs to be modified.
 * @param aEnd   Set to the highest memory address +1 which needs to be modified.

 @pre	Kernel must be locked.
 @pre	Call in a thread context.
 @pre	Interrupts must be enabled.
 */
EXPORT_C __NAKED__ void NKern::SchedulerHooks(TLinAddr& aStart, TLinAddr& aEnd)
	{
#if 0
	ASM_CHECK_PRECONDITIONS(MASK_INTERRUPTS_ENABLED|MASK_KERNEL_LOCKED|MASK_NOT_ISR|MASK_NOT_IDFC);
#ifdef __DEBUGGER_SUPPORT__
	asm("adr r2,resched_trampoline_hook_address");
	asm("str r2,[r0]");
	asm("adr r2,resched_trampoline_hook_address+4");
	asm("str r2,[r1]");
#else
	asm("mov r2,#0");
	asm("str r2,[r0]");
	asm("str r2,[r1]");
#endif
#endif
	__JUMP(,lr);
	};


/** 
 * Modifies the scheduler code so that it can call the function set by
 * NKern::SetRescheduleCallback().
 *
 * This requires that the region of memory indicated by NKern::SchedulerHooks() is writable.

 @pre	Kernel must be locked.
 @pre	Call in a thread context.
 @pre	Interrupts must be enabled.
 */
EXPORT_C __NAKED__ void NKern::InsertSchedulerHooks()
	{
#if 0
	ASM_CHECK_PRECONDITIONS(MASK_INTERRUPTS_ENABLED|MASK_KERNEL_LOCKED|MASK_NOT_ISR|MASK_NOT_IDFC);
#ifdef __DEBUGGER_SUPPORT__
	asm("adr r0,resched_trampoline_hook_address");
	asm("adr r1,resched_trampoline");
	asm("sub r1, r1, r0");
	asm("sub r1, r1, #8");
	asm("mov r1, r1, asr #2");
	asm("add r1, r1, #0xea000000");  // r1 = a branch instruction from resched_trampoline_hook_address to resched_trampoline

#if defined(__CPU_MEMORY_TYPE_REMAPPING)
	// These platforms have shadow memory in non-writable page. We cannot use the standard
	// Epoc::CopyToShadowMemory interface as we hold Kernel lock here.
	// Instead, we'll temporarily disable access permission checking in MMU by switching
	// domain#0 into Manager Mode (see Domain Access Control Register).
	asm("mrs r12, CPSR ");				// save cpsr setting and ...
	CPSIDAIF;							// ...disable interrupts
	asm("mrc p15, 0, r2, c3, c0, 0 ");	// read DACR
	asm("orr r3, r2, #3");				// domain #0 is the first two bits. manager mode is 11b
	asm("mcr p15, 0, r3, c3, c0, 0 ");	// write DACR
	asm("str r1,[r0]");
	asm("mcr p15, 0, r2, c3, c0, 0 ");	// write back the original value of DACR
	asm("msr CPSR_cxsf, r12 "); 		// restore cpsr setting (re-enable interrupts)
#else
	asm("str r1,[r0]");
#endif

#endif
#endif
	__JUMP(,lr);
	};


/** 
 * Reverts the modification of the Scheduler code performed by NKern::InsertSchedulerHooks()
 *
 * This requires that the region of memory indicated by NKern::SchedulerHooks() is writable.

 @pre	Kernel must be locked.
 @pre	Call in a thread context.
 @pre	Interrupts must be enabled.
 */
EXPORT_C __NAKED__ void NKern::RemoveSchedulerHooks()
	{
#if 0
	ASM_CHECK_PRECONDITIONS(MASK_INTERRUPTS_ENABLED|MASK_KERNEL_LOCKED|MASK_NOT_ISR|MASK_NOT_IDFC);
#ifdef __DEBUGGER_SUPPORT__
	asm("adr r0,resched_trampoline_hook_address");
	asm("ldr r1,resched_trampoline_unhook_data");

#if defined(__CPU_MEMORY_TYPE_REMAPPING)
	// See comments above in InsertSchedulerHooks
	asm("mrs r12, CPSR ");				// save cpsr setting and ...
	CPSIDAIF;							// ...disable interrupts
	asm("mrc p15, 0, r2, c3, c0, 0 ");	// read DACR
	asm("orr r3, r2, #3");				// domain #0 is the first two bits. manager mode is 11b
	asm("mcr p15, 0, r3, c3, c0, 0 ");	// write DACR
	asm("str r1,[r0]");
	asm("mcr p15, 0, r2, c3, c0, 0 ");	// write back the original value of DACR
	asm("msr CPSR_cxsf, r12 "); 		// restore cpsr setting (re-enable interrupts)
#else
	asm("str r1,[r0]");
#endif

#endif
#endif
	__JUMP(,lr);
	};


/** 
 * Set the function which is to be called on every thread reschedule.
 *
 * @param aCallback  Pointer to callback function, or NULL to disable callback.

 @pre	Kernel must be locked.
 @pre	Call in a thread context.
 @pre	Interrupts must be enabled.
 */
EXPORT_C __NAKED__ void NKern::SetRescheduleCallback(TRescheduleCallback /*aCallback*/)
	{
#if 0
	ASM_CHECK_PRECONDITIONS(MASK_INTERRUPTS_ENABLED|MASK_KERNEL_LOCKED|MASK_NOT_ISR|MASK_NOT_IDFC);
#ifdef __DEBUGGER_SUPPORT__
	asm("ldr r1, __TheScheduler ");
	asm("str r0, [r1, #%a0]" : : "i" _FOFF(TScheduler,iRescheduleHook));
#endif
#endif
	__JUMP(,lr);
	};



/** Disables interrupts to specified level.

	Note that if we are not disabling all interrupts we must lock the kernel
	here, otherwise a high priority interrupt which is still enabled could
	cause a reschedule and the new thread could then reenable interrupts.

	@param  aLevel Interrupts are disbabled up to and including aLevel.  On ARM,
			level 1 stands for IRQ only and level 2 stands for IRQ and FIQ.
	@return CPU-specific value passed to RestoreInterrupts.

	@pre 1 <= aLevel <= maximum level (CPU-specific)

	@see NKern::RestoreInterrupts()
 */
EXPORT_C __NAKED__ TInt NKern::DisableInterrupts(TInt /*aLevel*/)
	{
#ifdef __FIQ_IS_UNCONTROLLED__
	asm("mrs	r1, cpsr ");
	asm("cmp	r0, #0 ");
	asm("beq	1f ");
	__ASM_CLI();
	asm("1: ");
	asm("and	r0, r1, #%a0" : : "i" ((TInt)KAllInterruptsMask));	/* return original CPSR I and F bits */
	__JUMP(,	lr);
#else
	asm("cmp	r0, #1 ");
	asm("bhi "	CSM_ZN5NKern20DisableAllInterruptsEv);	// if level>1, disable all
	asm("mrs	r2, cpsr ");			// r2=original CPSR
	asm("bcc	1f ");					// skip if level=0
	__ASM_CLI();						// Disable all interrupts to prevent migration
	GET_RWNO_TID(,r12);					// r12 -> TSubScheduler
	asm("ldr	r3, [r12, #%a0]!" : : "i" _FOFF(TSubScheduler,iKernLockCount));
	asm("and	r0, r2, #0xc0 ");
	asm("cmp	r3, #0 ");				// test if kernel locked
	asm("addeq	r3, r3, #1 ");			// if not, lock the kernel
	asm("streq	r3, [r12] ");
	asm("orreq	r0, r0, #0x80000000 ");	// and set top bit to indicate kernel locked
	__ASM_STI2();						// reenable FIQs only
	__JUMP(,	lr);
	asm("1: ");
	asm("and	r0, r2, #0xc0 ");
	__JUMP(,	lr);
#endif
	}


/** Disables all interrupts (e.g. both IRQ and FIQ on ARM). 

	@return CPU-specific value passed to NKern::RestoreInterrupts().

	@see NKern::RestoreInterrupts()
 */
EXPORT_C __NAKED__ TInt NKern::DisableAllInterrupts()
	{
	asm("mrs r1, cpsr ");
	asm("and r0, r1, #%a0" : : "i" ((TInt)KAllInterruptsMask));	/* return original CPSR I and F bits */
	__ASM_CLI();
	__JUMP(,lr);
	}


/** Enables all interrupts (e.g. IRQ and FIQ on ARM).

	This function never unlocks the kernel.  So it must be used
	only to complement NKern::DisableAllInterrupts. Never use it
	to complement NKern::DisableInterrupts.

	@see NKern::DisableInterrupts()
	@see NKern::DisableAllInterrupts()

	@internalComponent
 */
EXPORT_C __NAKED__ void NKern::EnableAllInterrupts()
	{
	__ASM_STI();
	__JUMP(,lr);
	}


/** Restores interrupts to previous level and unlocks the kernel if it was 
	locked when disabling them.

	@param 	aRestoreData CPU-specific data returned from NKern::DisableInterrupts
			or NKern::DisableAllInterrupts specifying the previous interrupt level.

	@see NKern::DisableInterrupts()
	@see NKern::DisableAllInterrupts()
 */
EXPORT_C __NAKED__ void NKern::RestoreInterrupts(TInt /*aRestoreData*/)
	{
	asm("tst r0, r0 ");					// test state of top bit of aLevel
	asm("mrs r1, cpsr ");
	asm("and r0, r0, #%a0" : : "i" ((TInt)KAllInterruptsMask));
	asm("bic r1, r1, #%a0" : : "i" ((TInt)KAllInterruptsMask));
	asm("orr r1, r1, r0 ");				// replace I and F bits with those supplied
	asm("msr cpsr_c, r1 ");				// flags are unchanged (in particular N)
	__JUMP(pl,lr);						// if top bit of aLevel clear, finished

	// if top bit of aLevel set, fall through to unlock the kernel
	}


/**	Unlocks the kernel.

	Decrements iKernLockCount for current CPU; if it becomes zero and IDFCs
	or a reschedule are	pending, calls the scheduler to process them.
	Must be called in mode_svc.

    @pre    Call either in a thread or an IDFC context.
    @pre    Do not call from an ISR.
 */
EXPORT_C __NAKED__ void NKern::Unlock()
	{
	ASM_CHECK_PRECONDITIONS(MASK_NOT_ISR);

	GET_RWNO_TID(,r0)						// r0=&SubScheduler()
	__ASM_CLI();							// interrupts off
	asm("ldr	r1, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iKernLockCount));
	asm("ldr	r12, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iReschedIPIs));
	asm("ldr	r2, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iRescheduleNeededFlag));
	asm("subs	r3, r1, #1 ");
	asm("strne	r3, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iKernLockCount));
	asm("bne	0f ");						// kernel still locked -> return
	asm("cmp	r2, #0 ");					// check for DFCs or reschedule
	asm("bne	1f ");
	asm("cmp	r12, #0 ");					// IPIs outstanding?
	asm("str	r3, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iKernLockCount));	// unlock the kernel
	asm("bne	2f ");
	asm("0: ");
	__ASM_STI();							// interrupts back on
	__JUMP(,lr);

	// need to run IDFCs and/or reschedule
	asm("1: ");
	asm("stmfd	sp!, {r0,r4-r11,lr} ");
	asm("bl "	CSM_ZN10TScheduler10RescheduleEv );
	asm(".global nkern_unlock_resched_return ");
	asm("nkern_unlock_resched_return: ");

	// need to send any outstanding reschedule IPIs
	asm("cmp	r12, #0 ");
	asm("blne " CSM_CFUNC(send_accumulated_resched_ipis));
	asm("ldmfd	sp!, {r0,r4-r11,lr} ");
	__ASM_STI();
	__JUMP(,lr);

	asm("2:		");
	asm("stmfd	sp!, {r0,lr} ");
	asm("bl "	CSM_CFUNC(send_accumulated_resched_ipis));
	asm("ldmfd	sp!, {r0,lr} ");
	__ASM_STI();
	__JUMP(,lr);
	}


/**	Locks the kernel.

	Increments iKernLockCount for the current CPU, thereby deferring IDFCs
	and preemption.	Must be called in mode_svc.

    @pre    Call either in a thread or an IDFC context.
    @pre    Do not call from an ISR.
 */
EXPORT_C __NAKED__ void NKern::Lock()
	{
	ASM_CHECK_PRECONDITIONS(MASK_NOT_ISR);

	__ASM_CLI();
	GET_RWNO_TID(,r12);
	asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,iKernLockCount));
	asm("add r3, r3, #1 ");			// lock the kernel
	asm("str r3, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,iKernLockCount));
	__ASM_STI();
	__JUMP(,lr);
	}


/**	Locks the kernel and returns a pointer to the current thread.

	Increments iKernLockCount for the current CPU, thereby deferring IDFCs
	and preemption.	Must be called in mode_svc.

    @pre    Call either in a thread or an IDFC context.
    @pre    Do not call from an ISR.
 */
EXPORT_C __NAKED__ NThread* NKern::LockC()
	{
	ASM_CHECK_PRECONDITIONS(MASK_NOT_ISR);

	__ASM_CLI();
	GET_RWNO_TID(,r12);
	asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,iKernLockCount));
	asm("ldr r0, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,iCurrentThread));
	asm("add r3, r3, #1 ");			// lock the kernel
	asm("str r3, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,iKernLockCount));
	__ASM_STI();
	__JUMP(,lr);
	}


/**	Allows IDFCs and rescheduling if they are pending.

	If IDFCs or a reschedule are pending and iKernCSLocked is exactly equal to 1
	calls the scheduler to process the IDFCs and possibly reschedule.
	Must be called in mode_svc.

	@return	Nonzero if a reschedule actually occurred, zero if not.

    @pre    Call either in a thread or an IDFC context.
    @pre    Do not call from an ISR.
 */
EXPORT_C __NAKED__ TInt NKern::PreemptionPoint()
	{
	ASM_CHECK_PRECONDITIONS(MASK_NOT_ISR);

	GET_RWNO_TID(,r0)						// r0=&SubScheduler()
	asm("ldr	r1, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iKernLockCount));
	asm("ldr	r2, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iRescheduleNeededFlag));
	asm("ldr	r12, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iReschedIPIs));
	asm("cmp	r1, #1 ");
	asm("bgt	0f ");						// if locked more than once return FALSE
	asm("cmp	r2, #0 ");					// locked once and IDFCs/reschedule pending?
	asm("bne	1f ");						// skip if so
	asm("cmp	r12, #0 ");					// locked once and resched IPIs outstanding?
	asm("bne	2f ");						// skip if so
	asm("0:		");
	asm("mov	r0, #0 ");
	__JUMP(,	lr);						// else return FALSE

	// need to run IDFCs and/or reschedule
	asm("1:		");
	asm("stmfd	sp!, {r1,r4-r11,lr} ");
	asm("bl "	CSM_ZN10TScheduler10RescheduleEv );
	asm(".global nkern_preemption_point_resched_return ");
	asm("nkern_preemption_point_resched_return: ");
	asm("str	r2, [sp] ");
	asm("mov	r2, #1 ");
	asm("str	r2, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iKernLockCount));

	// need to send any outstanding reschedule IPIs
	asm("cmp	r12, #0 ");
	asm("blne " CSM_CFUNC(send_accumulated_resched_ipis));
	asm("ldmfd	sp!, {r0,r4-r11,lr} ");		// return TRUE if reschedule occurred
	__ASM_STI();
	__JUMP(,	lr);

	asm("2:		");
	asm("stmfd	sp!, {r2,lr} ");
	asm("bl "	CSM_CFUNC(send_accumulated_resched_ipis));
	asm("ldmfd	sp!, {r0,lr} ");			// return TRUE if reschedule occurred
	__ASM_STI();
	__JUMP(,	lr);
	}


#ifdef __CPU_HAS_VFP
// Do the actual VFP context save
__NAKED__ void VfpContextSave(void*)
	{
	VFP_FMRX(,1,VFP_XREG_FPEXC);
	asm("tst r1, #%a0" : : "i" ((TInt)VFP_FPEXC_EN) );		// Check to see if VFP in use
	__JUMP(eq, lr);											// Return immediately if not
	asm("tst r1, #%a0" : : "i" ((TInt)VFP_FPEXC_EX) );		// Check to see if an exception has occurred
	asm("beq 1f ");											// Skip ahead if not
	asm("bic r1, r1, #%a0" : : "i" ((TInt)VFP_FPEXC_EX));
	VFP_FMXR(,VFP_XREG_FPEXC,1);							// Reset exception flag
	asm("orr r1, r1, #%a0" : : "i" ((TInt)VFP_FPEXC_EX));	// But store it for later
	asm("1: ");


	VFP_FMRX(,2,VFP_XREG_FPSCR);
	asm("stmia	r0!, {r2} ");								// Save FPSCR

#ifndef __VFP_V3
	VFP_FMRX(,2,VFP_XREG_FPINST);
	VFP_FMRX(,3,VFP_XREG_FPINST2);
	asm("stmia	r0!, {r2-r3} ");							// Save FPINST, FPINST2
#endif

	VFP_FSTMIADW(CC_AL,0,0,16);								// Save D0 - D15

#ifdef __VFP_V3
	VFP_FMRX(,2,VFP_XREG_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_D32DIS));	// Check to see if access to the upper 16 registers is disabled
	VFP_FSTMIADW(CC_EQ,0,16,16);							// If not then save D16 - D31
#endif

	asm("0: ");
	asm("bic r1, r1, #%a0" : : "i" ((TInt)VFP_FPEXC_EN) );
	VFP_FMXR(,VFP_XREG_FPEXC,1);							// Disable VFP

	__JUMP(,lr);
	}
#endif


/** Check if the kernel is locked the specified number of times.

	@param aCount	The number of times the kernel should be locked
					If zero, tests if it is locked at all
	@return TRUE if the tested condition is true.

	@internalTechnology
*/
EXPORT_C __NAKED__ TBool NKern::KernelLocked(TInt /*aCount*/)
	{
	asm("mrs	r12, cpsr ");
	__ASM_CLI();
	GET_RWNO_TID(,r3);
	asm("movs	r1, r0 ");			// r1 = aCount
	asm("ldr	r0, [r3, #%a0]" : : "i" _FOFF(TSubScheduler, iKernLockCount));
	asm("moveq	r1, r0 ");			// if aCount=0, aCount=iKernLockCount
	asm("cmp	r1, r0 ");			//
	asm("movne	r0, #0 ");			// if aCount!=iKernLockCount, return FALSE else return iKernLockCount
	asm("msr	cpsr, r12 ");
	__JUMP(,lr);
	}


// Only call this if thread migration is disabled, i.e.
// interrupts disabled, kernel locked or current thread in 'freeze cpu' mode
extern "C" __NAKED__ TSubScheduler& SubScheduler()
	{
	GET_RWNO_TID(,r0);
	__JUMP(,lr);
	}

/** Returns the NThread control block for the currently scheduled thread.

    Note that this is the calling thread if called from a thread context, or the
	interrupted thread if called from an interrupt context.
	
	@return A pointer to the NThread for the currently scheduled thread.
	
	@pre Call in any context.
*/
EXPORT_C __NAKED__ NThread* NKern::CurrentThread()
	{
	asm("mrs	r12, cpsr ");
	__ASM_CLI();
	GET_RWNO_TID(,r0);
	asm("cmp	r0, #0 ");
	asm("ldrne	r0, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iCurrentThread));
	asm("msr	cpsr, r12 ");
	__JUMP(,lr);
	}


/** Returns the NThread control block for the currently scheduled thread.

    Note that this is the calling thread if called from a thread context, or the
	interrupted thread if called from an interrupt context.
	
	@return A pointer to the NThread for the currently scheduled thread.
	
	@pre Call with migration disabled - i.e. from an ISR, IDFC, with interrupts
			disabled or with preemption disabled.
*/
extern "C" __NAKED__ NThread* NCurrentThreadL()
	{
	GET_RWNO_TID(,r0);
	asm("ldr	r0, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iCurrentThread));
	__JUMP(,lr);
	}


/** Returns the CPU number of the calling CPU.

	@return the CPU number of the calling CPU.
	
	@pre Call in any context.
*/
EXPORT_C __NAKED__ TInt NKern::CurrentCpu()
	{
	asm("mrs	r12, cpsr ");
	__ASM_CLI();
	GET_RWNO_TID(,r0);
	asm("cmp	r0, #0 ");
	asm("ldrne	r0, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iCpuNum));
	asm("msr	cpsr, r12 ");
	__JUMP(,lr);
	}


/**	Returns the current processor context type (thread, IDFC or interrupt).

	@return	A value from NKern::TContext enumeration (but never EEscaped).
	
	@pre	Call in any context.

	@see	NKern::TContext
 */
EXPORT_C __NAKED__ TInt NKern::CurrentContext()
	{
	asm("mrs r1, cpsr ");
	__ASM_CLI();							// interrupts off to stop migration
	GET_RWNO_TID(,r3);						// r3 = &SubScheduler()
	asm("mov r0, #2 ");						// 2 = interrupt
	asm("and r2, r1, #0x1f ");				// r1 = mode
	asm("cmp r2, #0x13 ");
	asm("ldreqb r0, [r3, #%a0]" : : "i" _FOFF(TSubScheduler,iInIDFC));
	asm("bne 0f ");							// if not svc, must be interrupt
	asm("cmp r0, #0 ");
	asm("movne r0, #1 ");					// if iInIDFC, return 1 else return 0
	asm("0: ");
	asm("msr cpsr, r1 ");					// restore interrupts
	__JUMP(,lr);
	}


extern "C" __NAKED__ void send_irq_ipi(TSubScheduler*, TInt)
	{
	asm("tst	r1, #%a0" : : "i" ((TInt)EQueueEvent_WakeUp) );
	asm("bne "	CSM_CFUNC(wake_up_for_ipi));
	__DATA_SYNC_BARRIER_Z__(r3);			// need DSB before sending any IPI
	asm("ldr	r3, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, iCpuMask));
	asm("ldr	r2, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iGicDistAddr));	// we assume i_GicDistAddr is the same for all CPUs
	asm("mov	r1, #%a0" : : "i" ((TInt)TRANSFERRED_IRQ_VECTOR));
	asm("orr	r1, r1, r3, lsl #16 ");
	asm("str	r1, [r2, #%a0]" : : "i" _FOFF(GicDistributor, iSoftIrq));	// trigger IPIs
	__JUMP(,lr);
	}

// Send any outstanding reschedule IPIs when the kernel is unlocked on this CPU.
// Call with interrupts disabled, R0->TSubScheduler, R12=R0->iReschedIPIs
// Return with R0 unaltered.
extern "C" __NAKED__ void send_accumulated_resched_ipis()
	{
	asm("ldr	r3, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, iScheduler));
	asm("ldr	r2, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iGicDistAddr));
	asm("ldr	r1, [r3, #%a0]" : : "i" _FOFF(TScheduler, iThreadAcceptCpus));
	asm("bics	r1, r12, r1 ");
	asm("bne	2f ");
	asm("1:		");
	asm("mov	r1, #0 ");
	asm("str	r1, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iReschedIPIs));
	__DATA_SYNC_BARRIER__(r1);				// need DSB before sending any IPI
	asm("mov	r1, r12, lsl #16 ");
//	asm("orr	r1, r1, #%a0" : : "i" ((TInt)RESCHED_IPI_VECTOR));	RESCHED_IPI_VECTOR=0
	asm("str	r1, [r2, #%a0]" : : "i" _FOFF(GicDistributor, iSoftIrq));	// trigger IPIs
	__JUMP(,lr);

	asm("2:		");
	asm("stmfd	sp!, {r0,lr} ");
	asm("mov	r0, r3 ");
	asm("mov	r1, r12 ");
	asm("bl		ReschedInactiveCpus__10TSchedulerUl ");	// CSM needed
	asm("mov	r12, r0 ");
	asm("ldmfd	sp!, {r0,lr} ");
	asm("ldr	r2, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iGicDistAddr));
	asm("b		1b ");
	}

// Send a reschedule IPI to the specified CPU
extern "C" __NAKED__ void send_resched_ipi(TInt /*aCpu*/)
	{
	GET_RWNO_TID(,r3);
	__DATA_SYNC_BARRIER_Z__(r2);			// need DSB before sending any IPI
	asm("ldr	r2, [r3, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iGicDistAddr));	// we assume i_GicDistAddr is the same for all CPUs
	ASM_DEBUG1(SendReschedIPI,r0);
	asm("mov	r1, #0x10000 ");
	asm("mov	r1, r1, lsl r0 ");	// 0x10000<<aCpu
//	asm("orr	r1, r1, #%a0" : : "i" ((TInt)RESCHED_IPI_VECTOR));	RESCHED_IPI_VECTOR=0
	asm("str	r1, [r2, #%a0]" : : "i" _FOFF(GicDistributor, iSoftIrq));	// trigger IPIs
	__JUMP(,lr);
	}

// Send a reschedule IPI to the current processor
// *** DON'T DO ANY TRACING OR INSTRUMENTATION ***
extern "C" __NAKED__ void send_self_resched_ipi()
	{
	GET_RWNO_TID(,r3);
	__DATA_SYNC_BARRIER_Z__(r2);			// need DSB before sending any IPI
	asm("ldr	r2, [r3, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iGicDistAddr));	// we assume i_GicDistAddr is the same for all CPUs
	asm("mov	r1, #0x02000000 ");			// target = requesting CPU only
//	asm("orr	r1, r1, #%a0" : : "i" ((TInt)RESCHED_IPI_VECTOR));	RESCHED_IPI_VECTOR=0
	asm("str	r1, [r2, #%a0]" : : "i" _FOFF(GicDistributor, iSoftIrq));	// trigger IPI
	__JUMP(,lr);
	}

extern "C" __NAKED__ void send_resched_ipi_and_wait(TInt /*aCpu*/)
	{
	asm("ldr	r1, __TheSubSchedulers ");
	asm("mov	r2, #0x10000 ");
	asm("mov	r2, r2, lsl r0 ");	// 0x10000<<aCpu
	ASM_DEBUG1(SendReschedIPIAndWait,r0);
	asm("add	r0, r1, r0, lsl #%a0 " : : "i" ((TInt)KSubSchedulerShift));
	asm("ldr	r3, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iGicDistAddr));	// we assume i_GicDistAddr is the same for all CPUs
	asm("ldr	r12, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iIrqCount));
	__DATA_SYNC_BARRIER_Z__(r1);		// make sure i_IrqCount is read before IPI is sent
//	asm("orr	r2, r2, #%a0" : : "i" ((TInt)RESCHED_IPI_VECTOR));	RESCHED_IPI_VECTOR=0
	asm("str	r2, [r3, #%a0]" : : "i" _FOFF(GicDistributor, iSoftIrq));	// trigger IPIs
	__DATA_SYNC_BARRIER__(r1);			// make sure IPI has been sent
	asm("1: ");
	asm("ldrb	r1, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, iRescheduleNeededFlag));
	asm("ldr	r2, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iIrqNestCount));
	asm("ldr	r3, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iIrqCount));
	asm("cmp	r1, #0 ");
	asm("beq	0f ");					// iRescheduleNeededFlag not set -> wait
	asm("cmp	r2, #0 ");
	asm("bge	2f ");					// if other CPU is in an ISR, finish
	asm("cmp	r3, r12 ");				// if not, has i_IrqCount changed?
	asm("0: ");
	ARM_WFEcc(CC_EQ);					// if not, wait for something to happen ...
	asm("beq	1b ");					// ... and loop
	asm("2: ");
	__DATA_MEMORY_BARRIER__(r1);		// make sure subsequent memory accesses don't jump the gun
										// guaranteed to observe final thread state after this
	__JUMP(,lr);

	asm("__TheSubSchedulers: ");
	asm(".word TheSubSchedulers ");
	}


/*	If the current thread is subject to timeslicing, update its remaining time
	from the current CPU's local timer. Don't stop the timer.
	If the remaining time is negative, save it as zero.
 */
__NAKED__ void TSubScheduler::SaveTimesliceTimer(NThreadBase* /*aThread*/)
	{
	asm("ldr	r3, [r1, #%a0]" : : "i" _FOFF(NThreadBase,iTime));
	asm("ldrb	r12, [r1, #%a0]" : : "i" _FOFF(NThreadBase,i_NThread_Initial));
	asm("ldr	r2, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iLocalTimerAddr));
	asm("cmp	r3, #0 ");
	asm("ble	0f ");					// thread isn't timesliced or timeslice already expired so skip
	asm("cmp	r12, #0 ");
	asm("bne	0f ");					// initial (i.e. idle) thread, so skip
	asm("ldr	r3, [r2, #%a0]" : : "i" _FOFF(ArmLocalTimer,iTimerCount));
	asm("ldr	r12, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iTimerPeriodM));
	asm("ldr	r2, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iTimerPeriodS));
	asm("cmp	r3, #0 ");
	asm("movmi	r0, #0 ");				// if timer count is negative, save zero
	asm("bmi	1f ");
	asm("umull	r0, r3, r12, r3 ");		// scale up to max timer clock (R3:R0)
	asm("rsb	r12, r2, #32 ");
	asm("movs	r0, r0, lsr r2 ");		// r0 >>= iSSX.iTimerPeriodS, C = last bit shifted off (rounding)
	asm("orr	r0, r0, r3, lsl r12 ");	// bottom bits from r12 into top bits of r0
	asm("adcs	r0, r0, #0 ");			// round using last bit shifted off
	asm("1:		");
	asm("str	r0, [r1, #%a0]" : : "i" _FOFF(NThreadBase,iTime));
	asm("0:		");
	__JUMP(,lr);
	}


#if defined(__UTT_MACHINE_CODED__)
#if defined(__NKERN_TIMESTAMP_USE_LOCAL_TIMER__)
#error Use of local timer for NKern::Timestamp() no longer supported
#else

/*	Update aOld's execution time and set up the timer for aNew
	Update this CPU's timestamp value

	if (!aOld) aOld=iInitialThread
	if (!aNew) aNew=iInitialThread
	newcount = aNew->iTime>0 ? Max(aNew->iTime*iSSX.iTimerFreqM/2^(32+iTimerFreqS), 1) : 2^31-1
	cli()
	oldcount = timer count
	if (oldcount<=0 || aOld!=aNew)
		{
		timer count = newcount
		iSSX.iLastTimerSet = newcount
		if (aOld!=aNew)
			{
			TUint64 now = NKern::Timestamp();
			elapsed = iLastTimestamp -= now;
			iLastTimestamp = now;
			aOld->iTotalCpuTime.i64 += elapsed;
			if (!aOld->iActiveState)
				aOld->iTotalActiveTime.i64 += (now - aOld->iLastActivationTime.i64);
			++iReschedCount.i64;
			++aNew->iRunCount.i64;
			}
		}
	sti()
 */
__NAKED__ void TSubScheduler::UpdateThreadTimes(NThreadBase* /*aOld*/, NThreadBase* /*aNew*/)
	{
	asm("cmp	r2, #0 ");
	asm("ldreq	r2, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iInitialThread));
	asm("ldr	r12, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iTimerFreqM));
	asm("cmp	r1, #0 ");
	asm("ldreq	r1, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iInitialThread));
	asm("ldr	r3, [r2, #%a0]" : : "i" _FOFF(NThreadBase,iTime));
	asm("stmfd	sp!, {r4-r7} ");
	asm("cmp	r1, r2 ");
	asm("beq	2f ");
	asm("ldr	r6, [r2, #%a0]" : : "i" _FOFF(NThreadBase,iRunCount.i32[0]));
	asm("ldr	r7, [r2, #%a0]" : : "i" _FOFF(NThreadBase,iRunCount.i32[1]));
	asm("adds	r6, r6, #1 ");
	asm("str	r6, [r2, #%a0]" : : "i" _FOFF(NThreadBase,iRunCount.i32[0]));
	asm("ldr	r4, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iReschedCount.i32[0]));
	asm("ldr	r6, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iReschedCount.i32[1]));
	asm("adcs	r7, r7, #0 ");
	asm("str	r7, [r2, #%a0]" : : "i" _FOFF(NThreadBase,iRunCount.i32[1]));
	asm("adds	r4, r4, #1 ");
	asm("adcs	r6, r6, #0 ");
	asm("str	r4, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iReschedCount.i32[0]));
	asm("str	r6, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iReschedCount.i32[1]));
	asm("2:		");
	asm("ldr	r6, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iLocalTimerAddr));
	asm("cmp	r3, #1 ");					// aNew->iTime > 0 ?
	asm("movlt	r3, #0x7fffffff ");			// if not, use 2^31-1
	asm("blt	3f ");
	asm("cmp	r1, r2 ");					// different thread?
	asm("beq	0f ");						// no - finish
	asm("ldr	r5, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iTimerFreqS));
	asm("umull	r4, r3, r12, r3 ");			// r3:r4 = aNew->iTime * iTimerFreqM
	asm("adds	r4, r4, r4 ");				// bit 31 into C
	asm("teq	r5, #0 ");					// check for iTimerFreqS=0 without changing C
	asm("movnes	r3, r3, lsr r5 ");			// if not, r3>>=iTimerFreqS, last bit shifted out into C
	asm("adcs	r3, r3, #0 ");				// round using last bit shifted off
	asm("3:		");
	asm("str	r3, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iLastTimerSet));
	asm("str	r3, [r6, #%a0]" : : "i" _FOFF(ArmLocalTimer,iTimerCount));	// set new timeslice value in timer

	asm("ldr	r6, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iLastTimestamp.i32[0]));
	asm("ldr	r7, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iLastTimestamp.i32[1]));
	asm("stmfd	sp!, {r0-r2,lr} ");
	asm("bl	" CSM_ZN5NKern9TimestampEv);		// R1:R0 = current time
	asm("mov	r4, r0 ");
	asm("mov	r5, r1 ");					// R5:R4 = current time
	asm("ldmfd	sp!, {r0-r2,lr} ");
	asm("str	r4, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iLastTimestamp.i32[0]));
	asm("ldr	r3, [r1, #%a0]!" : : "i" _FOFF(NThreadBase,iTotalCpuTime.i64));
	asm("ldr	r12, [r1, #4] ");
	asm("str	r5, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iLastTimestamp.i32[1]));
	asm("stmdb	r1, {r4-r5} ");				// aOld->iLastRunTime
	asm("ldrb	r2, [r1, #%a0]" : : "i" (_FOFF(NSchedulable,iActiveState)-_FOFF(NThreadBase,iTotalCpuTime.i64)));
	asm("subs	r6, r4, r6 ");
	asm("sbcs	r7, r5, r7 ");				// R7:R6 = time since last reschedule
	asm("adds	r3, r3, r6 ");
	asm("adcs	r12, r12, r7 ");			// total CPU time of old thread
	asm("stmia	r1!, {r3,r12} ");			// store, r1=&aOld.iLastActivationTime
	asm("cmp	r2, #0 ");					// old thread still active?
	asm("bne	0f ");						// yes - done
	asm("ldmia	r1!, {r2,r3,r6,r7} ");		// R3:R2 = last activation time, R7:R6=total active time
	asm("subs	r2, r4, r2 ");
	asm("sbcs	r3, r5, r3 ");				// R3:R2 = time since last activation
	asm("adds	r6, r6, r2 ");
	asm("adcs	r7, r7, r3 ");				// R7:R6 = new total active time
	asm("stmdb	r1, {r6,r7} ");

	asm("0:		");
	asm("ldmfd	sp!, {r4-r7} ");
	__JUMP(,lr);
	}

#endif
#endif	// __UTT_MACHINE_CODED__