| branch | RCL_3 |
| changeset 43 | c1f20ce4abcf |
| parent 0 | a41df078684a |
| child 44 | 3e88ff8f41d5 |
| 42:a179b74831c9 | 43:c1f20ce4abcf |
|---|---|
17 |
17 |
18 #include <e32cia.h> |
18 #include <e32cia.h> |
19 #include <arm.h> |
19 #include <arm.h> |
20 #include <arm_gic.h> |
20 #include <arm_gic.h> |
21 #include <arm_tmr.h> |
21 #include <arm_tmr.h> |
22 |
|
23 |
22 |
24 |
23 |
25 __NAKED__ void Arm::GetUserSpAndLr(TAny*) |
24 __NAKED__ void Arm::GetUserSpAndLr(TAny*) |
26 { |
25 { |
27 asm("stmia r0, {r13, r14}^ "); |
26 asm("stmia r0, {r13, r14}^ "); |
285 __JUMP(, lr); |
284 __JUMP(, lr); |
286 } |
285 } |
287 #endif |
286 #endif |
288 |
287 |
289 |
288 |
290 |
289 #if defined(__NKERN_TIMESTAMP_USE_LOCAL_TIMER__) |
291 /** Get the current value of the system timestamp |
290 #error Use of local timer for NKern::Timestamp() no longer supported |
292 |
291 |
293 @publishedPartner |
292 #elif defined(__NKERN_TIMESTAMP_USE_SCU_GLOBAL_TIMER__) |
294 @prototype |
293 // Code to access global timer in Cortex A9 r1p0 |
295 */ |
294 |
295 #ifndef __CPU_ARM_HAS_GLOBAL_TIMER_BLOCK |
|
296 #error NKern::Timestamp() wants global timer, but global timer not present. |
|
297 #endif |
|
298 |
|
296 EXPORT_C __NAKED__ TUint64 NKern::Timestamp() |
299 EXPORT_C __NAKED__ TUint64 NKern::Timestamp() |
297 { |
300 { |
298 asm("ldr r3, __TheScheduler "); |
301 asm("ldr r3, __TheScheduler "); |
299 asm("mrs r12, cpsr "); // r12 = saved interrupt mask |
302 asm("mrs r12, cpsr "); // r12 = saved interrupt mask |
300 asm("ldr r2, [r3, #%a0]" : : "i" _FOFF(TScheduler,i_LocalTimerAddr)); // r2 points to local timer |
303 asm("stmfd sp!, {r4-r7} "); |
304 asm("ldr r4, [r3, #%a0]" : : "i" _FOFF(TScheduler,iSX.iGlobalTimerAddr)); // r4 points to global timer |
|
301 __ASM_CLI(); // disable all interrupts |
305 __ASM_CLI(); // disable all interrupts |
302 GET_RWNO_TID(,r3); // r3 -> TSubScheduler |
306 asm("ldr r6, [r3, #%a0]" : : "i" _FOFF(TScheduler,iSX.iCount0)); // r6 = count value of last frequency change (low) |
303 asm("ldr r1, [r2, #%a0]" : : "i" _FOFF(ArmLocalTimer,iTimerCount)); // r1 = current timer counter |
307 asm("ldr r7, [r3, #%a0]" : : "i" (_FOFF(TScheduler,iSX.iCount0)+4)); // r7 = count value of last frequency change (high) |
304 asm("ldr r0, [r3, #%a0]" : : "i" _FOFF(TSubScheduler,i_LastTimerSet)); // r0 = last value written to timer counter |
308 asm("ldr r2, [r4, #%a0]" : : "i" _FOFF(ArmGlobalTimer,iTimerCountHigh)); // r2 = current timer counter high word |
305 asm("ldr r2, [r3, #%a0]" : : "i" _FOFF(TSubScheduler,i_TimerMultI)); // r2 = scaling factor |
309 |
306 asm("sub r0, r0, r1 "); // elapsed timer ticks since last timestamp sync |
310 // To read 64 bit timer value, read high, low, high |
307 asm("umull r1, r2, r0, r2 "); // r2:r1 = elapsed ticks * scaling factor |
311 // If two high values match -> OK, else repeat |
308 asm("ldr r0, [r3, #%a0]!" : : "i" _FOFF(TSubScheduler,iLastTimestamp64)); // r0 = last timestamp sync point, low word |
312 asm("1: "); |
309 asm("ldr r3, [r3, #4] "); // r3 = last timestamp sync point, high word |
313 asm("mov r1, r2 "); // r1 = previous value of timer counter high word |
310 asm("adds r1, r1, #0x00800000 "); // add 2^23 (rounding) |
314 asm("ldr r0, [r4, #%a0]" : : "i" _FOFF(ArmGlobalTimer,iTimerCountLow)); // r0 = current timer counter low word |
311 asm("adcs r2, r2, #0 "); |
315 asm("ldr r2, [r4, #%a0]" : : "i" _FOFF(ArmGlobalTimer,iTimerCountHigh)); // r2 = current timer counter high word |
312 asm("mov r1, r1, lsr #24 "); // divide by 2^24 |
316 asm("mov r5, r3 "); // r5 = &TheScheduler |
313 asm("orr r1, r1, r2, lsl #8 "); // r1 = elapsed time since last timestamp sync |
317 asm("cmp r1, r2 "); // high word changed? |
318 asm("bne 1b "); // if so, retry |
|
319 |
|
320 // Now have R1:R0 = 64 bit global timer count |
|
321 asm("ldr r3, [r5, #%a0]" : : "i" _FOFF(TScheduler,iSX.iGTimerFreqRI.iI.iM)); // r3 = period multiplier |
|
322 asm("ldrsh r4, [r5, #%a0]" : : "i" _FOFF(TScheduler,iSX.iGTimerFreqRI.iI.iX)); // r4 = period multiplier shift |
|
323 asm("subs r6, r0, r6 "); // r7:r6 = ticks from last frequency change |
|
324 asm("sbcs r7, r1, r7 "); |
|
325 asm("umull r0, r1, r6, r3 "); |
|
326 asm("mov r2, #0 "); |
|
327 asm("umlal r1, r2, r7, r3 "); // r2:r1:r0 = delta * period multiplier |
|
328 asm("ldr r6, [r5, #%a0]!" : : "i" _FOFF(TScheduler,iSX.iTimestamp0)); // r6 = timestamp at last freq change (low) |
|
329 asm("ldr r7, [r5, #4] "); // r7 = timestamp at last freq change (high) |
|
314 asm("msr cpsr, r12 "); // restore interrupts |
330 asm("msr cpsr, r12 "); // restore interrupts |
315 asm("adds r0, r0, r1 "); // r1:r0 = last timestamp sync point + elapsed time since last timestamp sync |
331 asm("rsb r4, r4, #0 "); |
316 asm("adcs r1, r3, #0 "); |
332 asm("rsb r3, r4, #32 "); |
317 __JUMP(,lr); |
333 asm("movs r0, r0, lsr r4 "); // rounding bit into C |
334 asm("orr r0, r0, r1, lsl r3 "); |
|
335 asm("mov r1, r1, lsr r4 "); |
|
336 asm("orr r1, r1, r2, lsl r3 "); // r1:r0 = (delta * period multiplier) >> period multiplier shift |
|
337 asm("adcs r0, r0, r6 "); // scaled delta + timestamp at last freq change |
|
338 asm("adcs r1, r1, r7 "); |
|
339 asm("ldmfd sp!, {r4-r7} "); |
|
340 __JUMP(,lr); |
|
341 |
|
318 asm("__TheScheduler: "); |
342 asm("__TheScheduler: "); |
319 asm(".word %a0" : : "i" ((TInt)&TheScheduler)); |
343 asm(".word %a0" : : "i" ((TInt)&TheScheduler)); |
320 } |
344 } |
345 |
|
346 // Compensate for a change of frequency of the clocking driving the ARM Global Timer |
|
347 // Call with interrupts disabled |
|
348 __NAKED__ void ArmGlobalTimerFreqChg(const SRatioInv* /*aNewGTimerFreqRI*/) |
|
349 { |
|
350 asm("ldr r3, __TheScheduler "); |
|
351 asm("stmfd sp!, {r4-r7} "); |
|
352 asm("ldr r4, [r3, #%a0]" : : "i" _FOFF(TScheduler,iSX.iGlobalTimerAddr)); // r4 points to global timer |
|
353 asm("ldr r6, [r3, #%a0]" : : "i" _FOFF(TScheduler,iSX.iCount0)); // r6 = count value of last frequency change (low) |
|
354 asm("ldr r7, [r3, #%a0]" : : "i" (_FOFF(TScheduler,iSX.iCount0)+4)); // r7 = count value of last frequency change (high) |
|
355 asm("ldr r12, [r4, #%a0]" : : "i" _FOFF(ArmGlobalTimer,iTimerCountHigh)); // r12 = current timer counter high word |
|
356 asm("mov r5, r3 "); // r5 = &TheScheduler |
|
357 |
|
358 // To read 64 bit timer value, read high, low, high |
|
359 // If two high values match -> OK, else repeat |
|
360 asm("1: "); |
|
361 asm("mov r3, r12 "); // r3 = previous value of timer counter high word |
|
362 asm("ldr r2, [r4, #%a0]" : : "i" _FOFF(ArmGlobalTimer,iTimerCountLow)); // r0 = current timer counter low word |
|
363 asm("ldr r12, [r4, #%a0]" : : "i" _FOFF(ArmGlobalTimer,iTimerCountHigh)); // r12 = current timer counter high word |
|
364 asm("cmp r3, r12 "); // high word changed? |
|
365 asm("bne 1b "); // if so, retry |
|
366 |
|
367 // Now have R3:R2 = 64 bit global timer count |
|
368 asm("str r2, [r5, #%a0]" : : "i" _FOFF(TScheduler,iSX.iCount0)); // update count value at last frequency change |
|
369 asm("str r3, [r5, #%a0]" : : "i" (_FOFF(TScheduler,iSX.iCount0)+4)); // to be equal to current count value |
|
370 asm("subs r6, r2, r6 "); // r7:r6 = ticks (at old frequency) from last frequency change |
|
371 asm("sbcs r7, r3, r7 "); |
|
372 asm("ldr r3, [r5, #%a0]" : : "i" _FOFF(TScheduler,iSX.iGTimerFreqRI.iI.iM)); // r3 = old period multiplier |
|
373 asm("ldrsh r4, [r5, #%a0]" : : "i" _FOFF(TScheduler,iSX.iGTimerFreqRI.iI.iX)); // r4 = old period multiplier shift |
|
374 asm("ldmia r0, {r0,r1,r2,r12} "); // r1:r0=new frequency multiplier, r12:r2=new period multiplier |
|
375 asm("str r0, [r5, #%a0]" : : "i" _FOFF(TScheduler,iSX.iGTimerFreqRI.iR.iM)); // update frequency multiplier |
|
376 asm("str r1, [r5, #%a0]" : : "i" _FOFF(TScheduler,iSX.iGTimerFreqRI.iR.iX)); // update frequency multiplier |
|
377 asm("str r2, [r5, #%a0]" : : "i" _FOFF(TScheduler,iSX.iGTimerFreqRI.iI.iM)); // update period multiplier |
|
378 asm("str r12, [r5, #%a0]" : : "i" _FOFF(TScheduler,iSX.iGTimerFreqRI.iI.iX)); // update period multiplier |
|
379 asm("umull r0, r1, r6, r3 "); |
|
380 asm("mov r2, #0 "); |
|
381 asm("umlal r1, r2, r7, r3 "); // r2:r1:r0 = delta * old period multiplier |
|
382 asm("ldr r6, [r5, #%a0]!" : : "i" _FOFF(TScheduler,iSX.iTimestamp0)); // r6 = timestamp at last freq change (low) |
|
383 asm("ldr r7, [r5, #4] "); // r7 = timestamp at last freq change (high) |
|
384 asm("rsb r4, r4, #0 "); |
|
385 asm("rsb r3, r4, #32 "); |
|
386 asm("movs r0, r0, lsr r4 "); // rounding bit into C |
|
387 asm("orr r0, r0, r1, lsl r3 "); |
|
388 asm("mov r1, r1, lsr r4 "); |
|
389 asm("orr r1, r1, r2, lsl r3 "); // r1:r0 = (delta * old period multiplier) >> old period multiplier shift |
|
390 asm("adcs r0, r0, r6 "); // scaled delta + timestamp at last freq change |
|
391 asm("adcs r1, r1, r7 "); |
|
392 asm("stmia r5, {r0,r1} "); // timestamp at last freq change = now |
|
393 __DATA_MEMORY_BARRIER_Z__(r12); /* Ensure all updates visible */ |
|
394 asm("ldmfd sp!, {r4-r7} "); |
|
395 __JUMP(,lr); |
|
396 } |
|
397 |
|
398 #elif defined(__NKERN_TIMESTAMP_USE_INLINE_BSP_CODE__) |
|
399 #define __DEFINE_NKERN_TIMESTAMP_ASM__ |
|
400 #include <variant_timestamp.h> |
|
401 #undef __DEFINE_NKERN_TIMESTAMP_ASM__ |
|
402 #elif defined(__NKERN_TIMESTAMP_USE_BSP_CALLOUT__) |
|
403 // Code to call function defined in variant |
|
404 #else |
|
405 #error No definition for NKern::Timestamp() |
|
406 #endif |
|
321 |
407 |
322 |
408 |
323 extern "C" __NAKED__ TLinAddr get_sp_svc() |
409 extern "C" __NAKED__ TLinAddr get_sp_svc() |
324 { |
410 { |
325 asm("mrs r1, cpsr "); |
411 asm("mrs r1, cpsr "); |
357 asm("msr cpsr, r1 "); |
443 asm("msr cpsr, r1 "); |
358 __JUMP(, lr); |
444 __JUMP(, lr); |
359 |
445 |
360 asm("1: "); |
446 asm("1: "); |
361 GET_RWNO_TID(,r3); |
447 GET_RWNO_TID(,r3); |
362 asm("ldr r2, [r3, #%a0]" : : "i" _FOFF(TSubScheduler,i_FiqStackTop)); // if so, r2->top of FIQ stack |
448 asm("ldr r2, [r3, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iFiqStackTop)); // if so, r2->top of FIQ stack |
363 asm("ldr r0, [r2, #-4] "); // get return address |
449 asm("ldr r0, [r2, #-4] "); // get return address |
364 asm("msr cpsr, r1 "); |
450 asm("msr cpsr, r1 "); |
365 __JUMP(, lr); |
451 __JUMP(, lr); |
366 } |
452 } |
367 |
453 |
592 SPIN_LOCK_MARK_ACQ() |
678 SPIN_LOCK_MARK_ACQ() |
593 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
679 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
594 __JUMP(,lr); |
680 __JUMP(,lr); |
595 |
681 |
596 asm("2: "); |
682 asm("2: "); |
683 asm("ldr r1, __CrashState "); |
|
597 ARM_WFE; |
684 ARM_WFE; |
598 asm("ldrb r1, [r0, #0] "); /* read out count again */ |
685 asm("ldr r1, [r1] "); /* check for system crash while we were waiting */ |
599 asm("b 3b "); |
686 asm("cmp r1, #0 "); |
687 asm("bne 9f "); |
|
688 asm("ldrb r1, [r0, #0] "); /* read out count again */ |
|
689 asm("b 3b "); |
|
690 asm("9: "); |
|
691 __ASM_CRASH(); /* system crashed while we were waiting */ |
|
600 } |
692 } |
601 |
693 |
602 __NAKED__ EXPORT_C void TSpinLock::UnlockIrq() |
694 __NAKED__ EXPORT_C void TSpinLock::UnlockIrq() |
603 { |
695 { |
604 SPIN_UNLOCK_ENTRY_CHECK() |
696 SPIN_UNLOCK_ENTRY_CHECK() |
613 } |
705 } |
614 |
706 |
615 __NAKED__ EXPORT_C TBool TSpinLock::FlashIrq() |
707 __NAKED__ EXPORT_C TBool TSpinLock::FlashIrq() |
616 { |
708 { |
617 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
709 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
618 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,i_GicCpuIfcAddr)); |
710 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iGicCpuIfcAddr)); |
619 asm("ldrh r1, [r0, #0] "); |
711 asm("ldrh r1, [r0, #0] "); |
620 asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(GicCpuIfc,iHighestPending)); |
712 asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(GicCpuIfc,iHighestPending)); |
621 asm("sub r1, r1, r1, lsr #8 "); /* r1 low byte = (out - in) mod 256 */ |
713 asm("sub r1, r1, r1, lsr #8 "); /* r1 low byte = (out - in) mod 256 */ |
622 asm("and r1, r1, #0xFF "); |
714 asm("and r1, r1, #0xFF "); |
623 asm("cmp r1, #0xFF "); /* if out - in = -1, no-one else waiting */ |
715 asm("cmp r1, #0xFF "); /* if out - in = -1, no-one else waiting */ |
653 SPIN_LOCK_MARK_ACQ() |
745 SPIN_LOCK_MARK_ACQ() |
654 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
746 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
655 __JUMP(,lr); |
747 __JUMP(,lr); |
656 |
748 |
657 asm("2: "); |
749 asm("2: "); |
750 asm("ldr r1, __CrashState "); |
|
658 ARM_WFE; |
751 ARM_WFE; |
659 asm("ldrb r1, [r0, #0] "); /* read out count again */ |
752 asm("ldr r1, [r1] "); /* check for system crash while we were waiting */ |
660 asm("b 3b "); |
753 asm("cmp r1, #0 "); |
754 asm("bne 9f "); |
|
755 asm("ldrb r1, [r0, #0] "); /* read out count again */ |
|
756 asm("b 3b "); |
|
757 asm("9: "); |
|
758 __ASM_CRASH(); /* system crashed while we were waiting */ |
|
661 } |
759 } |
662 |
760 |
663 __NAKED__ EXPORT_C void TSpinLock::UnlockOnly() |
761 __NAKED__ EXPORT_C void TSpinLock::UnlockOnly() |
664 { |
762 { |
665 SPIN_UNLOCK_ENTRY_CHECK() |
763 SPIN_UNLOCK_ENTRY_CHECK() |
711 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
809 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
712 asm("and r0, r12, #%a0" : : "i" ((TInt)KAllInterruptsMask)); /* return original CPSR I and F bits */ |
810 asm("and r0, r12, #%a0" : : "i" ((TInt)KAllInterruptsMask)); /* return original CPSR I and F bits */ |
713 __JUMP(,lr); |
811 __JUMP(,lr); |
714 |
812 |
715 asm("2: "); |
813 asm("2: "); |
814 asm("ldr r1, __CrashState "); |
|
716 ARM_WFE; |
815 ARM_WFE; |
717 asm("ldrb r1, [r0, #0] "); /* read out count again */ |
816 asm("ldr r1, [r1] "); /* check for system crash while we were waiting */ |
718 asm("b 3b "); |
817 asm("cmp r1, #0 "); |
818 asm("bne 9f "); |
|
819 asm("ldrb r1, [r0, #0] "); /* read out count again */ |
|
820 asm("b 3b "); |
|
821 asm("9: "); |
|
822 __ASM_CRASH(); /* system crashed while we were waiting */ |
|
719 } |
823 } |
720 |
824 |
721 __NAKED__ EXPORT_C void TSpinLock::UnlockIrqRestore(TInt) |
825 __NAKED__ EXPORT_C void TSpinLock::UnlockIrqRestore(TInt) |
722 { |
826 { |
723 SPIN_UNLOCK_ENTRY_CHECK() |
827 SPIN_UNLOCK_ENTRY_CHECK() |
735 } |
839 } |
736 |
840 |
737 __NAKED__ EXPORT_C TBool TSpinLock::FlashIrqRestore(TInt) |
841 __NAKED__ EXPORT_C TBool TSpinLock::FlashIrqRestore(TInt) |
738 { |
842 { |
739 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
843 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
740 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,i_GicCpuIfcAddr)); |
844 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iGicCpuIfcAddr)); |
741 asm("ldrh r2, [r0, #0] "); |
845 asm("ldrh r2, [r0, #0] "); |
742 asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(GicCpuIfc,iHighestPending)); |
846 asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(GicCpuIfc,iHighestPending)); |
743 asm("sub r2, r2, r2, lsr #8 "); /* r2 low byte = (out - in) mod 256 */ |
847 asm("sub r2, r2, r2, lsr #8 "); /* r2 low byte = (out - in) mod 256 */ |
744 asm("and r2, r2, #0xFF "); |
848 asm("and r2, r2, #0xFF "); |
745 asm("cmp r2, #0xFF "); /* if out - in = -1, no-one else waiting */ |
849 asm("cmp r2, #0xFF "); /* if out - in = -1, no-one else waiting */ |
777 asm("bl " CSM_ZN5NKern15PreemptionPointEv); |
881 asm("bl " CSM_ZN5NKern15PreemptionPointEv); |
778 asm("ldr r0, [sp], #4 "); |
882 asm("ldr r0, [sp], #4 "); |
779 asm("bl " CSM_ZN9TSpinLock8LockOnlyEv); |
883 asm("bl " CSM_ZN9TSpinLock8LockOnlyEv); |
780 asm("mov r0, #1 "); |
884 asm("mov r0, #1 "); |
781 asm("ldr pc, [sp], #4 "); |
885 asm("ldr pc, [sp], #4 "); |
886 |
|
887 asm("__CrashState: "); |
|
888 asm(".word %a0" : : "i" ((TInt)&CrashState)); |
|
782 } |
889 } |
783 |
890 |
784 |
891 |
785 /****************************************************************************** |
892 /****************************************************************************** |
786 * Read/Write Spin lock |
893 * Read/Write Spin lock |
1159 RWSPIN_RLOCK_MARK_ACQ() |
1266 RWSPIN_RLOCK_MARK_ACQ() |
1160 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
1267 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
1161 __JUMP(,lr); |
1268 __JUMP(,lr); |
1162 |
1269 |
1163 asm("2: "); |
1270 asm("2: "); |
1271 asm("ldr r1, __CrashState "); |
|
1164 ARM_WFE; |
1272 ARM_WFE; |
1165 asm("ldr r1, [r0, #0] "); /* read out.w count again */ |
1273 asm("ldr r1, [r1] "); /* check for system crash while we were waiting */ |
1166 asm("b 3b "); |
1274 asm("cmp r1, #0 "); |
1275 asm("bne 9f "); |
|
1276 asm("ldr r1, [r0, #0] "); /* read out.w count again */ |
|
1277 asm("b 3b "); |
|
1278 asm("9: "); |
|
1279 __ASM_CRASH(); /* system crashed while we were waiting */ |
|
1167 } |
1280 } |
1168 |
1281 |
1169 __NAKED__ EXPORT_C void TRWSpinLock::UnlockIrqR() |
1282 __NAKED__ EXPORT_C void TRWSpinLock::UnlockIrqR() |
1170 { |
1283 { |
1171 RWSPIN_RUNLOCK_ENTRY_CHECK() |
1284 RWSPIN_RUNLOCK_ENTRY_CHECK() |
1183 } |
1296 } |
1184 |
1297 |
1185 __NAKED__ EXPORT_C TBool TRWSpinLock::FlashIrqR() |
1298 __NAKED__ EXPORT_C TBool TRWSpinLock::FlashIrqR() |
1186 { |
1299 { |
1187 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
1300 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
1188 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,i_GicCpuIfcAddr)); |
1301 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iGicCpuIfcAddr)); |
1189 asm("ldr r2, [r0, #0] "); |
1302 asm("ldr r2, [r0, #0] "); |
1190 asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(GicCpuIfc,iHighestPending)); |
1303 asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(GicCpuIfc,iHighestPending)); |
1191 asm("eor r2, r2, r2, lsr #16 "); /* r2 low byte = out.w ^ in.w = 0 if no writers waiting */ |
1304 asm("eor r2, r2, r2, lsr #16 "); /* r2 low byte = out.w ^ in.w = 0 if no writers waiting */ |
1192 asm("tst r2, #0xFF "); |
1305 asm("tst r2, #0xFF "); |
1193 asm("addeq r3, r3, #1 "); |
1306 asm("addeq r3, r3, #1 "); |
1228 RWSPIN_WLOCK_MARK_ACQ() |
1341 RWSPIN_WLOCK_MARK_ACQ() |
1229 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
1342 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
1230 __JUMP(,lr); |
1343 __JUMP(,lr); |
1231 |
1344 |
1232 asm("2: "); |
1345 asm("2: "); |
1346 asm("ldr r1, __CrashState "); |
|
1233 ARM_WFE; |
1347 ARM_WFE; |
1234 asm("ldr r1, [r0, #0] "); /* read out count again */ |
1348 asm("ldr r1, [r1] "); /* check for system crash while we were waiting */ |
1235 asm("b 3b "); |
1349 asm("cmp r1, #0 "); |
1350 asm("bne 9f "); |
|
1351 asm("ldr r1, [r0, #0] "); /* read out count again */ |
|
1352 asm("b 3b "); |
|
1353 asm("9: "); |
|
1354 __ASM_CRASH(); /* system crashed while we were waiting */ |
|
1236 } |
1355 } |
1237 |
1356 |
1238 __NAKED__ EXPORT_C void TRWSpinLock::UnlockIrqW() |
1357 __NAKED__ EXPORT_C void TRWSpinLock::UnlockIrqW() |
1239 { |
1358 { |
1240 RWSPIN_WUNLOCK_ENTRY_CHECK() |
1359 RWSPIN_WUNLOCK_ENTRY_CHECK() |
1249 } |
1368 } |
1250 |
1369 |
1251 __NAKED__ EXPORT_C TBool TRWSpinLock::FlashIrqW() |
1370 __NAKED__ EXPORT_C TBool TRWSpinLock::FlashIrqW() |
1252 { |
1371 { |
1253 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
1372 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
1254 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,i_GicCpuIfcAddr)); |
1373 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iGicCpuIfcAddr)); |
1255 asm("ldr r2, [r0, #0] "); |
1374 asm("ldr r2, [r0, #0] "); |
1256 asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(GicCpuIfc,iHighestPending)); |
1375 asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(GicCpuIfc,iHighestPending)); |
1257 asm("add r2, r2, #0x00010000 "); /* increment out.w */ |
1376 asm("add r2, r2, #0x00010000 "); /* increment out.w */ |
1258 asm("tst r2, #0x00FF0000 "); /* if wraparound, revert carry */ |
1377 asm("tst r2, #0x00FF0000 "); /* if wraparound, revert carry */ |
1259 asm("subeq r2, r2, #0x01000000 "); |
1378 asm("subeq r2, r2, #0x01000000 "); |
1298 RWSPIN_RLOCK_MARK_ACQ() |
1417 RWSPIN_RLOCK_MARK_ACQ() |
1299 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
1418 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
1300 __JUMP(,lr); |
1419 __JUMP(,lr); |
1301 |
1420 |
1302 asm("2: "); |
1421 asm("2: "); |
1422 asm("ldr r1, __CrashState "); |
|
1303 ARM_WFE; |
1423 ARM_WFE; |
1304 asm("ldr r1, [r0, #0] "); /* read out.w count again */ |
1424 asm("ldr r1, [r1] "); /* check for system crash while we were waiting */ |
1305 asm("b 3b "); |
1425 asm("cmp r1, #0 "); |
1426 asm("bne 9f "); |
|
1427 asm("ldr r1, [r0, #0] "); /* read out.w count again */ |
|
1428 asm("b 3b "); |
|
1429 asm("9: "); |
|
1430 __ASM_CRASH(); /* system crashed while we were waiting */ |
|
1306 } |
1431 } |
1307 |
1432 |
1308 __NAKED__ EXPORT_C void TRWSpinLock::UnlockOnlyR() |
1433 __NAKED__ EXPORT_C void TRWSpinLock::UnlockOnlyR() |
1309 { |
1434 { |
1310 RWSPIN_RUNLOCK_ENTRY_CHECK() |
1435 RWSPIN_RUNLOCK_ENTRY_CHECK() |
1360 RWSPIN_WLOCK_MARK_ACQ() |
1485 RWSPIN_WLOCK_MARK_ACQ() |
1361 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
1486 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
1362 __JUMP(,lr); |
1487 __JUMP(,lr); |
1363 |
1488 |
1364 asm("2: "); |
1489 asm("2: "); |
1490 asm("ldr r1, __CrashState "); |
|
1365 ARM_WFE; |
1491 ARM_WFE; |
1366 asm("ldr r1, [r0, #0] "); /* read out count again */ |
1492 asm("ldr r1, [r1] "); /* check for system crash while we were waiting */ |
1367 asm("b 3b "); |
1493 asm("cmp r1, #0 "); |
1494 asm("bne 9f "); |
|
1495 asm("ldr r1, [r0, #0] "); /* read out count again */ |
|
1496 asm("b 3b "); |
|
1497 asm("9: "); |
|
1498 __ASM_CRASH(); /* system crashed while we were waiting */ |
|
1368 } |
1499 } |
1369 |
1500 |
1370 __NAKED__ EXPORT_C void TRWSpinLock::UnlockOnlyW() |
1501 __NAKED__ EXPORT_C void TRWSpinLock::UnlockOnlyW() |
1371 { |
1502 { |
1372 RWSPIN_WUNLOCK_ENTRY_CHECK() |
1503 RWSPIN_WUNLOCK_ENTRY_CHECK() |
1426 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
1557 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
1427 asm("and r0, r12, #%a0" : : "i" ((TInt)KAllInterruptsMask)); /* return original CPSR I and F bits */ |
1558 asm("and r0, r12, #%a0" : : "i" ((TInt)KAllInterruptsMask)); /* return original CPSR I and F bits */ |
1428 __JUMP(,lr); |
1559 __JUMP(,lr); |
1429 |
1560 |
1430 asm("2: "); |
1561 asm("2: "); |
1562 asm("ldr r1, __CrashState "); |
|
1431 ARM_WFE; |
1563 ARM_WFE; |
1432 asm("ldr r1, [r0, #0] "); /* read out.w count again */ |
1564 asm("ldr r1, [r1] "); /* check for system crash while we were waiting */ |
1433 asm("b 3b "); |
1565 asm("cmp r1, #0 "); |
1566 asm("bne 9f "); |
|
1567 asm("ldr r1, [r0, #0] "); /* read out.w count again */ |
|
1568 asm("b 3b "); |
|
1569 asm("9: "); |
|
1570 __ASM_CRASH(); /* system crashed while we were waiting */ |
|
1434 } |
1571 } |
1435 |
1572 |
1436 __NAKED__ EXPORT_C void TRWSpinLock::UnlockIrqRestoreR(TInt) |
1573 __NAKED__ EXPORT_C void TRWSpinLock::UnlockIrqRestoreR(TInt) |
1437 { |
1574 { |
1438 RWSPIN_RUNLOCK_ENTRY_CHECK() |
1575 RWSPIN_RUNLOCK_ENTRY_CHECK() |
1453 } |
1590 } |
1454 |
1591 |
1455 __NAKED__ EXPORT_C TBool TRWSpinLock::FlashIrqRestoreR(TInt) |
1592 __NAKED__ EXPORT_C TBool TRWSpinLock::FlashIrqRestoreR(TInt) |
1456 { |
1593 { |
1457 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
1594 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
1458 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,i_GicCpuIfcAddr)); |
1595 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iGicCpuIfcAddr)); |
1459 asm("ldr r2, [r0, #0] "); |
1596 asm("ldr r2, [r0, #0] "); |
1460 asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(GicCpuIfc,iHighestPending)); |
1597 asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(GicCpuIfc,iHighestPending)); |
1461 asm("eor r2, r2, r2, lsr #16 "); /* r2 low byte = out.w ^ in.w = 0 if no writers waiting */ |
1598 asm("eor r2, r2, r2, lsr #16 "); /* r2 low byte = out.w ^ in.w = 0 if no writers waiting */ |
1462 asm("tst r2, #0xFF "); |
1599 asm("tst r2, #0xFF "); |
1463 asm("addeq r3, r3, #1 "); |
1600 asm("addeq r3, r3, #1 "); |
1500 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
1637 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
1501 asm("and r0, r12, #%a0" : : "i" ((TInt)KAllInterruptsMask)); /* return original CPSR I and F bits */ |
1638 asm("and r0, r12, #%a0" : : "i" ((TInt)KAllInterruptsMask)); /* return original CPSR I and F bits */ |
1502 __JUMP(,lr); |
1639 __JUMP(,lr); |
1503 |
1640 |
1504 asm("2: "); |
1641 asm("2: "); |
1642 asm("ldr r1, __CrashState "); |
|
1505 ARM_WFE; |
1643 ARM_WFE; |
1506 asm("ldr r1, [r0, #0] "); /* read out count again */ |
1644 asm("ldr r1, [r1] "); /* check for system crash while we were waiting */ |
1507 asm("b 3b "); |
1645 asm("cmp r1, #0 "); |
1646 asm("bne 9f "); |
|
1647 asm("ldr r1, [r0, #0] "); /* read out count again */ |
|
1648 asm("b 3b "); |
|
1649 asm("9: "); |
|
1650 __ASM_CRASH(); /* system crashed while we were waiting */ |
|
1508 } |
1651 } |
1509 |
1652 |
1510 __NAKED__ EXPORT_C void TRWSpinLock::UnlockIrqRestoreW(TInt) |
1653 __NAKED__ EXPORT_C void TRWSpinLock::UnlockIrqRestoreW(TInt) |
1511 { |
1654 { |
1512 RWSPIN_WUNLOCK_ENTRY_CHECK() |
1655 RWSPIN_WUNLOCK_ENTRY_CHECK() |
1524 } |
1667 } |
1525 |
1668 |
1526 __NAKED__ EXPORT_C TBool TRWSpinLock::FlashIrqRestoreW(TInt) |
1669 __NAKED__ EXPORT_C TBool TRWSpinLock::FlashIrqRestoreW(TInt) |
1527 { |
1670 { |
1528 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
1671 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
1529 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,i_GicCpuIfcAddr)); |
1672 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iGicCpuIfcAddr)); |
1530 asm("ldr r2, [r0, #0] "); |
1673 asm("ldr r2, [r0, #0] "); |
1531 asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(GicCpuIfc,iHighestPending)); |
1674 asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(GicCpuIfc,iHighestPending)); |
1532 asm("add r2, r2, #0x00010000 "); /* increment out.w */ |
1675 asm("add r2, r2, #0x00010000 "); /* increment out.w */ |
1533 asm("tst r2, #0x00FF0000 "); /* if wraparound, revert carry */ |
1676 asm("tst r2, #0x00FF0000 "); /* if wraparound, revert carry */ |
1534 asm("subeq r2, r2, #0x01000000 "); |
1677 asm("subeq r2, r2, #0x01000000 "); |