diff -r 000000000000 -r bde4ae8d615e os/kernelhwsrv/kernel/eka/nkernsmp/arm/ncsched.cia --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/os/kernelhwsrv/kernel/eka/nkernsmp/arm/ncsched.cia Fri Jun 15 03:10:57 2012 +0200 @@ -0,0 +1,1064 @@ +// 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 +#include +#include "nkern.h" +#include +#include +#include +//#include +//#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(); + + +__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,i_LocalTimerAddr)); // 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*) + { + __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, i_GicDistAddr)); // 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 r2, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, i_GicDistAddr)); + 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); + } + +// 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, i_GicDistAddr)); // 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< 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,i_LocalTimerAddr)); + 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,i_TimerMultI)); + 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 + asm("adds r0, r0, #0x00800000 "); + asm("adcs r3, r3, #0 "); + asm("mov r0, r0, lsr #24 "); + asm("orr r0, r0, r3, lsl #8 "); + asm("1: "); + asm("str r0, [r1, #%a0]" : : "i" _FOFF(NThreadBase,iTime)); + asm("0: "); + __JUMP(,lr); + } + +/* 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*i_TimerMultF/2^32, 1) : 2^31-1 + cli() + oldcount = timer count + if (oldcount<=0 || aOld!=aNew) + { + timer count = newcount + elapsed = i_LastTimerSet - oldcount + i_LastTimerSet = newcount + elapsed = elapsed * i_TimerMultI / 2^24 + aOld->iTotalCpuTime64 += elapsed + correction = i_TimestampError; + if (correction > i_MaxCorrection) + correction = i_MaxCorrection + else if (correction < -i_MaxCorrection) + correction = -i_MaxCorrection + i_TimestampError -= correction + i_LastTimestamp += elapsed + i_TimerGap - correction + } + 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,i_TimerMultF)); + 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("ldr r6, [r2, #%a0]" : : "i" _FOFF(NThreadBase,iRunCount64)); + asm("ldr r7, [r2, #%a0]" : : "i" (_FOFF(NThreadBase,iRunCount64)+4)); + asm("cmp r1, r2 "); + asm("beq 2f "); + asm("adds r6, r6, #1 "); + asm("str r6, [r2, #%a0]" : : "i" _FOFF(NThreadBase,iRunCount64)); + asm("ldr r4, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iReschedCount64)); + asm("ldr r6, [r0, #%a0]" : : "i" (_FOFF(TSubScheduler,iReschedCount64)+4)); + asm("adcs r7, r7, #0 "); + asm("str r7, [r2, #%a0]" : : "i" (_FOFF(NThreadBase,iRunCount64)+4)); + asm("adds r4, r4, #1 "); + asm("adcs r6, r6, #0 "); + asm("str r4, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iReschedCount64)); + asm("str r6, [r0, #%a0]" : : "i" (_FOFF(TSubScheduler,iReschedCount64)+4)); + asm("2: "); + asm("cmp r3, #1 "); // aNew->iTime > 0 ? + asm("umullge r4, r3, r12, r3 "); + asm("ldr r5, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,i_LocalTimerAddr)); + asm("movlt r3, #0x7fffffff "); + asm("addges r3, r3, r4, lsr #31 "); // round up top 32 bits if bit 31 set + asm("moveq r3, #1 "); // if result zero, limit to 1 + asm("ldr r12, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,i_LastTimerSet)); + __ASM_CLI(); + asm("ldr r4, [r5, #%a0]" : : "i" _FOFF(ArmLocalTimer,iTimerCount)); + asm("cmp r1, r2 "); + asm("bne 1f "); + asm("cmp r4, #0 "); + asm("bgt 0f "); // same thread, timeslice not expired -> leave timer alone + asm("1: "); + asm("str r3, [r5, #%a0]" : : "i" _FOFF(ArmLocalTimer,iTimerCount)); // set new timeslice value in timer + asm("ldr r5, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,i_TimerMultI)); + asm("str r3, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,i_LastTimerSet)); + asm("sub r12, r12, r4 "); // r12 = elapsed (actual timer ticks) + asm("umull r4, r5, r12, r5 "); + asm("ldr r3, [r1, #%a0]!" : : "i" _FOFF(NThreadBase,iTotalCpuTime64)); + asm("ldr r12, [r1, #4] "); + asm("adds r4, r4, #0x00800000 "); + asm("adcs r5, r5, #0 "); + asm("mov r4, r4, lsr #24 "); + asm("orr r4, r4, r5, lsl #8 "); // r4 = elapsed + asm("adds r3, r3, r4 "); + asm("adcs r12, r12, #0 "); + asm("stmia r1, {r3,r12} "); // aOld->iTotalCpuTime64 += elapsed + asm("ldr r3, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,i_TimestampError)); + asm("ldr r5, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,i_MaxCorrection)); + asm("ldr r1, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iLastTimestamp64)); + asm("ldr r2, [r0, #%a0]" : : "i" (_FOFF(TSubScheduler,iLastTimestamp64)+4)); + asm("mov r12, r3 "); + asm("cmp r3, r5 "); + asm("movgt r3, r5 "); // if (correction>i_MaxCorrection) correction=i_MaxCorrection + asm("cmn r3, r5 "); + asm("rsblt r3, r5, #0 "); // if (correction+i_MaxCorrection<0) correction=-i_MaxCorrection + asm("ldr r5, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,i_TimerGap)); + asm("sub r12, r12, r3 "); + asm("str r12, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,i_TimestampError)); + asm("add r4, r4, r5 "); // r4 = elapsed + i_TimerGap + asm("adds r1, r1, r4 "); + asm("adcs r2, r2, #0 "); // iLastTimestamp64 + (elapsed + i_TimerGap) + asm("subs r1, r1, r3 "); + asm("sbcs r1, r1, r3, asr #32 "); // iLastTimestamp64 + (elapsed + i_TimerGap - correction) + asm("str r1, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,iLastTimestamp64)); + asm("str r2, [r0, #%a0]" : : "i" (_FOFF(TSubScheduler,iLastTimestamp64)+4)); + asm("0: "); + __ASM_STI(); + asm("ldmfd sp!, {r4-r7} "); + __JUMP(,lr); + } + +