188 |
188 |
189 __NAKED__ void initialiseState(TInt /*aCpu*/, TSubScheduler* /*aSS*/) |
189 __NAKED__ void initialiseState(TInt /*aCpu*/, TSubScheduler* /*aSS*/) |
190 { |
190 { |
191 SET_RWNO_TID(,r1); |
191 SET_RWNO_TID(,r1); |
192 __ASM_CLI_MODE(MODE_ABT); |
192 __ASM_CLI_MODE(MODE_ABT); |
193 asm("str sp, [r1, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iAbtStackTop)); |
193 asm("str sp, [r1, #%a0]" : : "i" _FOFF(TSubScheduler, i_AbtStackTop)); |
194 asm("mvn r3, #0 "); |
194 asm("mvn r3, #0 "); |
195 asm("str r3, [sp, #%a0]" : : "i" _FOFF(SFullArmRegSet, iExcCode)); |
195 asm("str r3, [sp, #%a0]" : : "i" _FOFF(SFullArmRegSet, iExcCode)); |
196 asm("str r3, [r1, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iIrqNestCount)); |
196 asm("str r3, [r1, #%a0]" : : "i" _FOFF(TSubScheduler, i_IrqNestCount)); |
197 __ASM_CLI_MODE(MODE_UND); |
197 __ASM_CLI_MODE(MODE_UND); |
198 asm("str sp, [r1, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iUndStackTop)); |
198 asm("str sp, [r1, #%a0]" : : "i" _FOFF(TSubScheduler, i_UndStackTop)); |
199 __ASM_CLI_MODE(MODE_FIQ); |
199 __ASM_CLI_MODE(MODE_FIQ); |
200 asm("str sp, [r1, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iFiqStackTop)); |
200 asm("str sp, [r1, #%a0]" : : "i" _FOFF(TSubScheduler, i_FiqStackTop)); |
201 __ASM_CLI_MODE(MODE_IRQ); |
201 __ASM_CLI_MODE(MODE_IRQ); |
202 asm("str sp, [r1, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iIrqStackTop)); |
202 asm("str sp, [r1, #%a0]" : : "i" _FOFF(TSubScheduler, i_IrqStackTop)); |
203 __ASM_CLI_MODE(MODE_SVC); |
203 __ASM_CLI_MODE(MODE_SVC); |
204 asm("ldr r2, __TheScheduler "); |
204 asm("ldr r2, __TheScheduler "); |
205 asm("ldr r3, [r2, #%a0]" : : "i" _FOFF(TScheduler, iSX.iScuAddr)); |
205 asm("ldr r3, [r2, #%a0]" : : "i" _FOFF(TScheduler, i_ScuAddr)); |
206 asm("str r3, [r1, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iScuAddr)); |
206 asm("str r3, [r1, #%a0]" : : "i" _FOFF(TSubScheduler, i_ScuAddr)); |
207 asm("ldr r3, [r2, #%a0]" : : "i" _FOFF(TScheduler, iSX.iGicDistAddr)); |
207 asm("ldr r3, [r2, #%a0]" : : "i" _FOFF(TScheduler, i_GicDistAddr)); |
208 asm("str r3, [r1, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iGicDistAddr)); |
208 asm("str r3, [r1, #%a0]" : : "i" _FOFF(TSubScheduler, i_GicDistAddr)); |
209 asm("ldr r3, [r2, #%a0]" : : "i" _FOFF(TScheduler, iSX.iGicCpuIfcAddr)); |
209 asm("ldr r3, [r2, #%a0]" : : "i" _FOFF(TScheduler, i_GicCpuIfcAddr)); |
210 asm("str r3, [r1, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iGicCpuIfcAddr)); |
210 asm("str r3, [r1, #%a0]" : : "i" _FOFF(TSubScheduler, i_GicCpuIfcAddr)); |
211 asm("ldr r3, [r2, #%a0]" : : "i" _FOFF(TScheduler, iSX.iLocalTimerAddr)); |
211 asm("ldr r3, [r2, #%a0]" : : "i" _FOFF(TScheduler, i_LocalTimerAddr)); |
212 asm("str r3, [r1, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iLocalTimerAddr)); |
212 asm("str r3, [r1, #%a0]" : : "i" _FOFF(TSubScheduler, i_LocalTimerAddr)); |
213 asm("ldr r3, [r2, #%a0]" : : "i" _FOFF(TScheduler, iSX.iGlobalTimerAddr)); |
|
214 asm("str r3, [r1, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iGlobalTimerAddr)); |
|
215 asm("mov r3, #0 "); |
213 asm("mov r3, #0 "); |
216 SET_RWRO_TID(,r3); |
214 SET_RWRO_TID(,r3); |
217 SET_RWRW_TID(,r3); |
215 SET_RWRW_TID(,r3); |
218 |
216 |
219 __JUMP(,lr); |
217 __JUMP(,lr); |
238 { |
236 { |
239 asm("stmfd sp!, {r0-r1} "); // save parameters |
237 asm("stmfd sp!, {r0-r1} "); // save parameters |
240 GET_RWNO_TID(,r0); |
238 GET_RWNO_TID(,r0); |
241 asm("cmp r0, #0 "); |
239 asm("cmp r0, #0 "); |
242 asm("ldreq r0, __SS0 "); |
240 asm("ldreq r0, __SS0 "); |
243 asm("ldr r0, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iRegs)); |
241 asm("ldr r0, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,i_Regs)); |
244 asm("cmp r0, #0 "); |
242 asm("cmp r0, #0 "); |
245 asm("ldreq r0, __DefaultRegs "); |
243 asm("ldreq r0, __DefaultRegs "); |
246 asm("ldr r1, [r0, #%a0]" : : "i" _FOFF(SFullArmRegSet, iExcCode)); |
244 asm("ldr r1, [r0, #%a0]" : : "i" _FOFF(SFullArmRegSet, iExcCode)); |
247 asm("cmp r1, #0 "); // context already saved? |
245 asm("cmp r1, #0 "); // context already saved? |
248 asm("bge state_already_saved "); // skip if so |
246 asm("bge state_already_saved "); // skip if so |
275 asm("cmp r0, #0 "); |
273 asm("cmp r0, #0 "); |
276 asm("moveq r2, #1 "); |
274 asm("moveq r2, #1 "); |
277 asm("streq r2, [r1] "); |
275 asm("streq r2, [r1] "); |
278 asm("beq skip_other_cores "); // If subscheduler not yet set, don't bother with other cores |
276 asm("beq skip_other_cores "); // If subscheduler not yet set, don't bother with other cores |
279 asm("ldr r2, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, iCpuMask)); |
277 asm("ldr r2, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, iCpuMask)); |
280 asm("ldr r5, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iGicCpuIfcAddr)); |
278 asm("ldr r5, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, i_GicCpuIfcAddr)); |
281 // asm("ldr r4, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iRegs)); |
279 // asm("ldr r4, [r0, #%a0]" : : "i" _FOFF(TSubScheduler,i_Regs)); |
282 asm("bic sp, sp, #4 "); // align stack to multiple of 8 |
280 asm("bic sp, sp, #4 "); // align stack to multiple of 8 |
283 |
281 |
284 __DATA_MEMORY_BARRIER_Z__(r6); |
282 __DATA_MEMORY_BARRIER_Z__(r6); |
285 asm("1: "); |
283 asm("1: "); |
286 LDREX(3,1); |
284 LDREX(3,1); |
293 asm("beq first_to_crash "); // branch if so |
291 asm("beq first_to_crash "); // branch if so |
294 |
292 |
295 // we weren't first to crash, so wait here for a crash IPI |
293 // we weren't first to crash, so wait here for a crash IPI |
296 // disable all interrupts except for CRASH_IPI |
294 // disable all interrupts except for CRASH_IPI |
297 GET_RWNO_TID(,r0); |
295 GET_RWNO_TID(,r0); |
298 asm("ldr r0, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iGicCpuIfcAddr)); |
296 asm("ldr r0, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, i_GicCpuIfcAddr)); |
299 asm("mov r1, #0 "); |
297 asm("mov r1, #0 "); |
300 asm("1: "); |
298 asm("1: "); |
301 asm("add r1, r1, #1 "); |
299 asm("add r1, r1, #1 "); |
302 asm("str r1, [r0, #%a0]" : : "i" _FOFF(GicCpuIfc, iPriMask)); |
300 asm("str r1, [r0, #%a0]" : : "i" _FOFF(GicCpuIfc, iPriMask)); |
303 __DATA_SYNC_BARRIER__(r6); |
301 __DATA_SYNC_BARRIER__(r6); |
312 |
310 |
313 // This CPU was first to crash |
311 // This CPU was first to crash |
314 asm("first_to_crash: "); |
312 asm("first_to_crash: "); |
315 asm("ldr r2, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, iScheduler)); |
313 asm("ldr r2, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, iScheduler)); |
316 asm("ldr r7, __CrashStateOut "); |
314 asm("ldr r7, __CrashStateOut "); |
317 asm("ldr r3, [r2, #%a0]" : : "i" _FOFF(TScheduler, iIpiAcceptCpus)); |
315 asm("ldr r3, [r2, #%a0]" : : "i" _FOFF(TScheduler, iActiveCpus1)); |
318 asm("str r3, [r7] "); // mask of CPUs pending |
316 asm("str r3, [r7] "); // mask of CPUs pending |
319 asm("ldr r5, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iGicDistAddr)); |
317 asm("ldr r5, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, i_GicDistAddr)); |
320 asm("ldr r1, __CrashIPIWord "); |
318 asm("ldr r1, __CrashIPIWord "); |
321 __DATA_SYNC_BARRIER_Z__(r6); |
319 __DATA_SYNC_BARRIER_Z__(r6); |
322 asm("str r1, [r5, #%a0]" : : "i" _FOFF(GicDistributor, iSoftIrq)); // send CRASH_IPI to all other CPUs |
320 asm("str r1, [r5, #%a0]" : : "i" _FOFF(GicDistributor, iSoftIrq)); // send CRASH_IPI to all other CPUs |
323 __DATA_SYNC_BARRIER__(r6); |
321 __DATA_SYNC_BARRIER__(r6); |
324 |
322 |
1036 __NAKED__ EXPORT_C TBool BTrace::OutFilteredPcFormatBig(TUint32 a0, TUint32 aModuleUid, TUint32 aPc, TUint16 aFormatId, const TAny* aData, TInt aDataSize) |
1034 __NAKED__ EXPORT_C TBool BTrace::OutFilteredPcFormatBig(TUint32 a0, TUint32 aModuleUid, TUint32 aPc, TUint16 aFormatId, const TAny* aData, TInt aDataSize) |
1037 { |
1035 { |
1038 asm("mov r0, #0"); //Kernel side not implemented yet |
1036 asm("mov r0, #0"); //Kernel side not implemented yet |
1039 } |
1037 } |
1040 |
1038 |
1041 #ifdef __CPU_ARM_HAS_WFE_SEV |
1039 |
1042 |
1040 |
1043 extern "C" __NAKED__ void __arm_wfe() |
1041 |
1044 { |
1042 |
1045 ARM_WFE; |
|
1046 __JUMP(, lr); |
|
1047 } |
|
1048 |
|
1049 extern "C" __NAKED__ void __arm_sev() |
|
1050 { |
|
1051 ARM_SEV; |
|
1052 __JUMP(, lr); |
|
1053 } |
|
1054 |
|
1055 #endif |
|
1056 |
|
1057 // Called by a CPU which has completed its detach sequence and should now be powered off |
|
1058 // Doesn't return - just waits for power to be removed |
|
1059 // CPU will come back up via the reset vector when it next wakes up. |
|
1060 // NOTE: On entry the CPU caches are disabled and the CPU does not participate in coherency |
|
1061 // SO BE VERY CAREFUL |
|
1062 extern "C" __NAKED__ void DetachComplete() |
|
1063 { |
|
1064 GET_RWNO_TID(,r0); |
|
1065 asm("ldr r1, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, iUncached)); |
|
1066 asm("ldr r2, [r1, #%a0]" : : "i" _FOFF(SPerCpuUncached, iDetachCompleteCpus)); |
|
1067 asm("ldr r3, [r0, #%a0]" : : "i" _FOFF(TSubScheduler, iSSX.iGicDistAddr)); |
|
1068 __DATA_SYNC_BARRIER_Z__(r12); // need DSB before sending any IPI |
|
1069 asm("mov r2, r2, lsl #16 "); |
|
1070 asm("orr r2, r2, #%a0" : : "i" ((TInt)INDIRECT_POWERDOWN_IPI_VECTOR)); |
|
1071 asm("str r2, [r3, #%a0]" : : "i" _FOFF(GicDistributor, iSoftIrq)); // trigger IPIs |
|
1072 |
|
1073 asm("wait_forever: "); |
|
1074 __DATA_SYNC_BARRIER__(r12); |
|
1075 ARM_WFE; |
|
1076 __DATA_SYNC_BARRIER__(r12); |
|
1077 asm("ldr r2, [r1, #%a0]" : : "i" _FOFF(SPerCpuUncached, iPowerOnReq)); |
|
1078 __DATA_SYNC_BARRIER__(r12); |
|
1079 asm("cmp r2, #0xF000000F "); // for 'fake' power down |
|
1080 asm("bne wait_forever "); |
|
1081 |
|
1082 asm("0: "); |
|
1083 __JUMP(,lr); |
|
1084 } |
|
1085 |
|
1086 |
|
1087 |
|
1088 |
|
1089 |
|