| changeset 90 | 947f0dc9f7a8 |
| parent 0 | a41df078684a |
| child 177 | a232af6b0b1f |
| 52:2d65c2f76d7b | 90:947f0dc9f7a8 |
|---|---|
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 |
22 |
23 |
23 extern "C" { |
24 extern SVariantInterfaceBlock* VIB; |
|
25 } |
|
24 |
26 |
25 __NAKED__ void Arm::GetUserSpAndLr(TAny*) |
27 __NAKED__ void Arm::GetUserSpAndLr(TAny*) |
26 { |
28 { |
27 asm("stmia r0, {r13, r14}^ "); |
29 asm("stmia r0, {r13, r14}^ "); |
28 asm("mov r0, r0"); // NOP needed between stm^ and banked register access |
30 asm("mov r0, r0"); // NOP needed between stm^ and banked register access |
285 __JUMP(, lr); |
287 __JUMP(, lr); |
286 } |
288 } |
287 #endif |
289 #endif |
288 |
290 |
289 |
291 |
290 |
292 #if defined(__NKERN_TIMESTAMP_USE_LOCAL_TIMER__) |
291 /** Get the current value of the system timestamp |
293 #error Use of local timer for NKern::Timestamp() no longer supported |
292 |
294 |
293 @publishedPartner |
295 #elif defined(__NKERN_TIMESTAMP_USE_SCU_GLOBAL_TIMER__) |
294 @prototype |
296 // Code to access global timer in Cortex A9 r1p0 |
295 */ |
297 |
298 #ifndef __CPU_ARM_HAS_GLOBAL_TIMER_BLOCK |
|
299 #error NKern::Timestamp() wants global timer, but global timer not present. |
|
300 #endif |
|
301 |
|
296 EXPORT_C __NAKED__ TUint64 NKern::Timestamp() |
302 EXPORT_C __NAKED__ TUint64 NKern::Timestamp() |
297 { |
303 { |
298 asm("ldr r3, __TheScheduler "); |
304 asm("ldr r3, __TheScheduler "); |
299 asm("mrs r12, cpsr "); // r12 = saved interrupt mask |
305 asm("mrs r12, cpsr "); // r12 = saved interrupt mask |
300 asm("ldr r2, [r3, #%a0]" : : "i" _FOFF(TScheduler,i_LocalTimerAddr)); // r2 points to local timer |
306 asm("stmfd sp!, {r4-r7} "); |
307 asm("ldr r5, [r3, #%a0]" : : "i" _FOFF(TScheduler,iSub[0])); // r5->subscheduler for CPU0 |
|
308 asm("ldr r4, [r3, #%a0]" : : "i" _FOFF(TScheduler,iSX.iGlobalTimerAddr)); // r4 points to global timer |
|
301 __ASM_CLI(); // disable all interrupts |
309 __ASM_CLI(); // disable all interrupts |
302 GET_RWNO_TID(,r3); // r3 -> TSubScheduler |
310 asm("ldr r6, [r5, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iTicksSinceLastSync)); // r6 = count value of last frequency change (low) |
303 asm("ldr r1, [r2, #%a0]" : : "i" _FOFF(ArmLocalTimer,iTimerCount)); // r1 = current timer counter |
311 asm("ldr r7, [r5, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iLastTimerSet)); // 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 |
312 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 |
313 |
306 asm("sub r0, r0, r1 "); // elapsed timer ticks since last timestamp sync |
314 // To read 64 bit timer value, read high, low, high |
307 asm("umull r1, r2, r0, r2 "); // r2:r1 = elapsed ticks * scaling factor |
315 // 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 |
316 asm("1: "); |
309 asm("ldr r3, [r3, #4] "); // r3 = last timestamp sync point, high word |
317 asm("mov r1, r2 "); // r1 = previous value of timer counter high word |
310 asm("adds r1, r1, #0x00800000 "); // add 2^23 (rounding) |
318 asm("ldr r0, [r4, #%a0]" : : "i" _FOFF(ArmGlobalTimer,iTimerCountLow)); // r0 = current timer counter low word |
311 asm("adcs r2, r2, #0 "); |
319 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 |
320 asm("cmp r1, r2 "); // high word changed? |
313 asm("orr r1, r1, r2, lsl #8 "); // r1 = elapsed time since last timestamp sync |
321 asm("bne 1b "); // if so, retry |
322 |
|
323 // Now have R1:R0 = 64 bit global timer count |
|
324 asm("ldr r3, [r5, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iNTimerPeriodM)); // r3 = period multiplier |
|
325 asm("ldr r4, [r5, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iNTimerPeriodS)); // r4 = period multiplier shift |
|
326 asm("subs r6, r0, r6 "); // r7:r6 = ticks from last frequency change |
|
327 asm("sbcs r7, r1, r7 "); |
|
328 asm("umull r0, r1, r6, r3 "); |
|
329 asm("mov r2, #0 "); |
|
330 asm("umlal r1, r2, r7, r3 "); // r2:r1:r0 = delta * period multiplier |
|
331 asm("rsb r3, r4, #32 "); |
|
332 asm("ldr r6, [r5, #%a0]!" : : "i" _FOFF(TSubScheduler,iSSX.iLastSyncTime)); // r6 = timestamp at last freq change (low) |
|
333 asm("ldr r7, [r5, #4] "); // r7 = timestamp at last freq change (high) |
|
314 asm("msr cpsr, r12 "); // restore interrupts |
334 asm("msr cpsr, r12 "); // restore interrupts |
315 asm("adds r0, r0, r1 "); // r1:r0 = last timestamp sync point + elapsed time since last timestamp sync |
335 asm("movs r0, r0, lsr r4 "); // rounding bit into C |
316 asm("adcs r1, r3, #0 "); |
336 asm("orr r0, r0, r1, lsl r3 "); |
317 __JUMP(,lr); |
337 asm("mov r1, r1, lsr r4 "); |
338 asm("orr r1, r1, r2, lsl r3 "); // r1:r0 = (delta * period multiplier) >> period multiplier shift |
|
339 asm("adcs r0, r0, r6 "); // scaled delta + timestamp at last freq change |
|
340 asm("adcs r1, r1, r7 "); |
|
341 asm("ldmfd sp!, {r4-r7} "); |
|
342 __JUMP(,lr); |
|
343 |
|
318 asm("__TheScheduler: "); |
344 asm("__TheScheduler: "); |
319 asm(".word %a0" : : "i" ((TInt)&TheScheduler)); |
345 asm(".word %a0" : : "i" ((TInt)&TheScheduler)); |
320 } |
346 } |
347 |
|
348 #elif defined(__NKERN_TIMESTAMP_USE_INLINE_BSP_CODE__) |
|
349 #define __DEFINE_NKERN_TIMESTAMP_ASM__ |
|
350 #include <variant_timestamp.h> |
|
351 #undef __DEFINE_NKERN_TIMESTAMP_ASM__ |
|
352 #elif defined(__NKERN_TIMESTAMP_USE_BSP_CALLOUT__) |
|
353 // Code to call function defined in variant |
|
354 #else |
|
355 #error No definition for NKern::Timestamp() |
|
356 #endif |
|
321 |
357 |
322 |
358 |
323 extern "C" __NAKED__ TLinAddr get_sp_svc() |
359 extern "C" __NAKED__ TLinAddr get_sp_svc() |
324 { |
360 { |
325 asm("mrs r1, cpsr "); |
361 asm("mrs r1, cpsr "); |
357 asm("msr cpsr, r1 "); |
393 asm("msr cpsr, r1 "); |
358 __JUMP(, lr); |
394 __JUMP(, lr); |
359 |
395 |
360 asm("1: "); |
396 asm("1: "); |
361 GET_RWNO_TID(,r3); |
397 GET_RWNO_TID(,r3); |
362 asm("ldr r2, [r3, #%a0]" : : "i" _FOFF(TSubScheduler,i_FiqStackTop)); // if so, r2->top of FIQ stack |
398 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 |
399 asm("ldr r0, [r2, #-4] "); // get return address |
364 asm("msr cpsr, r1 "); |
400 asm("msr cpsr, r1 "); |
365 __JUMP(, lr); |
401 __JUMP(, lr); |
366 } |
402 } |
367 |
403 |
592 SPIN_LOCK_MARK_ACQ() |
628 SPIN_LOCK_MARK_ACQ() |
593 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
629 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
594 __JUMP(,lr); |
630 __JUMP(,lr); |
595 |
631 |
596 asm("2: "); |
632 asm("2: "); |
633 asm("ldr r1, __CrashState "); |
|
597 ARM_WFE; |
634 ARM_WFE; |
598 asm("ldrb r1, [r0, #0] "); /* read out count again */ |
635 asm("ldr r1, [r1] "); /* check for system crash while we were waiting */ |
599 asm("b 3b "); |
636 asm("cmp r1, #0 "); |
637 asm("bne 9f "); |
|
638 asm("ldrb r1, [r0, #0] "); /* read out count again */ |
|
639 asm("b 3b "); |
|
640 asm("9: "); |
|
641 __ASM_CRASH(); /* system crashed while we were waiting */ |
|
600 } |
642 } |
601 |
643 |
602 __NAKED__ EXPORT_C void TSpinLock::UnlockIrq() |
644 __NAKED__ EXPORT_C void TSpinLock::UnlockIrq() |
603 { |
645 { |
604 SPIN_UNLOCK_ENTRY_CHECK() |
646 SPIN_UNLOCK_ENTRY_CHECK() |
613 } |
655 } |
614 |
656 |
615 __NAKED__ EXPORT_C TBool TSpinLock::FlashIrq() |
657 __NAKED__ EXPORT_C TBool TSpinLock::FlashIrq() |
616 { |
658 { |
617 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
659 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
618 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,i_GicCpuIfcAddr)); |
660 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iGicCpuIfcAddr)); |
619 asm("ldrh r1, [r0, #0] "); |
661 asm("ldrh r1, [r0, #0] "); |
620 asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(GicCpuIfc,iHighestPending)); |
662 asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(GicCpuIfc,iHighestPending)); |
621 asm("sub r1, r1, r1, lsr #8 "); /* r1 low byte = (out - in) mod 256 */ |
663 asm("sub r1, r1, r1, lsr #8 "); /* r1 low byte = (out - in) mod 256 */ |
622 asm("and r1, r1, #0xFF "); |
664 asm("and r1, r1, #0xFF "); |
623 asm("cmp r1, #0xFF "); /* if out - in = -1, no-one else waiting */ |
665 asm("cmp r1, #0xFF "); /* if out - in = -1, no-one else waiting */ |
653 SPIN_LOCK_MARK_ACQ() |
695 SPIN_LOCK_MARK_ACQ() |
654 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
696 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
655 __JUMP(,lr); |
697 __JUMP(,lr); |
656 |
698 |
657 asm("2: "); |
699 asm("2: "); |
700 asm("ldr r1, __CrashState "); |
|
658 ARM_WFE; |
701 ARM_WFE; |
659 asm("ldrb r1, [r0, #0] "); /* read out count again */ |
702 asm("ldr r1, [r1] "); /* check for system crash while we were waiting */ |
660 asm("b 3b "); |
703 asm("cmp r1, #0 "); |
704 asm("bne 9f "); |
|
705 asm("ldrb r1, [r0, #0] "); /* read out count again */ |
|
706 asm("b 3b "); |
|
707 asm("9: "); |
|
708 __ASM_CRASH(); /* system crashed while we were waiting */ |
|
661 } |
709 } |
662 |
710 |
663 __NAKED__ EXPORT_C void TSpinLock::UnlockOnly() |
711 __NAKED__ EXPORT_C void TSpinLock::UnlockOnly() |
664 { |
712 { |
665 SPIN_UNLOCK_ENTRY_CHECK() |
713 SPIN_UNLOCK_ENTRY_CHECK() |
711 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
759 __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 */ |
760 asm("and r0, r12, #%a0" : : "i" ((TInt)KAllInterruptsMask)); /* return original CPSR I and F bits */ |
713 __JUMP(,lr); |
761 __JUMP(,lr); |
714 |
762 |
715 asm("2: "); |
763 asm("2: "); |
764 asm("ldr r1, __CrashState "); |
|
716 ARM_WFE; |
765 ARM_WFE; |
717 asm("ldrb r1, [r0, #0] "); /* read out count again */ |
766 asm("ldr r1, [r1] "); /* check for system crash while we were waiting */ |
718 asm("b 3b "); |
767 asm("cmp r1, #0 "); |
768 asm("bne 9f "); |
|
769 asm("ldrb r1, [r0, #0] "); /* read out count again */ |
|
770 asm("b 3b "); |
|
771 asm("9: "); |
|
772 __ASM_CRASH(); /* system crashed while we were waiting */ |
|
719 } |
773 } |
720 |
774 |
721 __NAKED__ EXPORT_C void TSpinLock::UnlockIrqRestore(TInt) |
775 __NAKED__ EXPORT_C void TSpinLock::UnlockIrqRestore(TInt) |
722 { |
776 { |
723 SPIN_UNLOCK_ENTRY_CHECK() |
777 SPIN_UNLOCK_ENTRY_CHECK() |
735 } |
789 } |
736 |
790 |
737 __NAKED__ EXPORT_C TBool TSpinLock::FlashIrqRestore(TInt) |
791 __NAKED__ EXPORT_C TBool TSpinLock::FlashIrqRestore(TInt) |
738 { |
792 { |
739 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
793 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
740 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,i_GicCpuIfcAddr)); |
794 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iGicCpuIfcAddr)); |
741 asm("ldrh r2, [r0, #0] "); |
795 asm("ldrh r2, [r0, #0] "); |
742 asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(GicCpuIfc,iHighestPending)); |
796 asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(GicCpuIfc,iHighestPending)); |
743 asm("sub r2, r2, r2, lsr #8 "); /* r2 low byte = (out - in) mod 256 */ |
797 asm("sub r2, r2, r2, lsr #8 "); /* r2 low byte = (out - in) mod 256 */ |
744 asm("and r2, r2, #0xFF "); |
798 asm("and r2, r2, #0xFF "); |
745 asm("cmp r2, #0xFF "); /* if out - in = -1, no-one else waiting */ |
799 asm("cmp r2, #0xFF "); /* if out - in = -1, no-one else waiting */ |
777 asm("bl " CSM_ZN5NKern15PreemptionPointEv); |
831 asm("bl " CSM_ZN5NKern15PreemptionPointEv); |
778 asm("ldr r0, [sp], #4 "); |
832 asm("ldr r0, [sp], #4 "); |
779 asm("bl " CSM_ZN9TSpinLock8LockOnlyEv); |
833 asm("bl " CSM_ZN9TSpinLock8LockOnlyEv); |
780 asm("mov r0, #1 "); |
834 asm("mov r0, #1 "); |
781 asm("ldr pc, [sp], #4 "); |
835 asm("ldr pc, [sp], #4 "); |
836 |
|
837 asm("__CrashState: "); |
|
838 asm(".word %a0" : : "i" ((TInt)&CrashState)); |
|
782 } |
839 } |
783 |
840 |
784 |
841 |
785 /****************************************************************************** |
842 /****************************************************************************** |
786 * Read/Write Spin lock |
843 * Read/Write Spin lock |
1159 RWSPIN_RLOCK_MARK_ACQ() |
1216 RWSPIN_RLOCK_MARK_ACQ() |
1160 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
1217 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
1161 __JUMP(,lr); |
1218 __JUMP(,lr); |
1162 |
1219 |
1163 asm("2: "); |
1220 asm("2: "); |
1221 asm("ldr r1, __CrashState "); |
|
1164 ARM_WFE; |
1222 ARM_WFE; |
1165 asm("ldr r1, [r0, #0] "); /* read out.w count again */ |
1223 asm("ldr r1, [r1] "); /* check for system crash while we were waiting */ |
1166 asm("b 3b "); |
1224 asm("cmp r1, #0 "); |
1225 asm("bne 9f "); |
|
1226 asm("ldr r1, [r0, #0] "); /* read out.w count again */ |
|
1227 asm("b 3b "); |
|
1228 asm("9: "); |
|
1229 __ASM_CRASH(); /* system crashed while we were waiting */ |
|
1167 } |
1230 } |
1168 |
1231 |
1169 __NAKED__ EXPORT_C void TRWSpinLock::UnlockIrqR() |
1232 __NAKED__ EXPORT_C void TRWSpinLock::UnlockIrqR() |
1170 { |
1233 { |
1171 RWSPIN_RUNLOCK_ENTRY_CHECK() |
1234 RWSPIN_RUNLOCK_ENTRY_CHECK() |
1183 } |
1246 } |
1184 |
1247 |
1185 __NAKED__ EXPORT_C TBool TRWSpinLock::FlashIrqR() |
1248 __NAKED__ EXPORT_C TBool TRWSpinLock::FlashIrqR() |
1186 { |
1249 { |
1187 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
1250 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
1188 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,i_GicCpuIfcAddr)); |
1251 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iGicCpuIfcAddr)); |
1189 asm("ldr r2, [r0, #0] "); |
1252 asm("ldr r2, [r0, #0] "); |
1190 asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(GicCpuIfc,iHighestPending)); |
1253 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 */ |
1254 asm("eor r2, r2, r2, lsr #16 "); /* r2 low byte = out.w ^ in.w = 0 if no writers waiting */ |
1192 asm("tst r2, #0xFF "); |
1255 asm("tst r2, #0xFF "); |
1193 asm("addeq r3, r3, #1 "); |
1256 asm("addeq r3, r3, #1 "); |
1228 RWSPIN_WLOCK_MARK_ACQ() |
1291 RWSPIN_WLOCK_MARK_ACQ() |
1229 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
1292 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
1230 __JUMP(,lr); |
1293 __JUMP(,lr); |
1231 |
1294 |
1232 asm("2: "); |
1295 asm("2: "); |
1296 asm("ldr r1, __CrashState "); |
|
1233 ARM_WFE; |
1297 ARM_WFE; |
1234 asm("ldr r1, [r0, #0] "); /* read out count again */ |
1298 asm("ldr r1, [r1] "); /* check for system crash while we were waiting */ |
1235 asm("b 3b "); |
1299 asm("cmp r1, #0 "); |
1300 asm("bne 9f "); |
|
1301 asm("ldr r1, [r0, #0] "); /* read out count again */ |
|
1302 asm("b 3b "); |
|
1303 asm("9: "); |
|
1304 __ASM_CRASH(); /* system crashed while we were waiting */ |
|
1236 } |
1305 } |
1237 |
1306 |
1238 __NAKED__ EXPORT_C void TRWSpinLock::UnlockIrqW() |
1307 __NAKED__ EXPORT_C void TRWSpinLock::UnlockIrqW() |
1239 { |
1308 { |
1240 RWSPIN_WUNLOCK_ENTRY_CHECK() |
1309 RWSPIN_WUNLOCK_ENTRY_CHECK() |
1249 } |
1318 } |
1250 |
1319 |
1251 __NAKED__ EXPORT_C TBool TRWSpinLock::FlashIrqW() |
1320 __NAKED__ EXPORT_C TBool TRWSpinLock::FlashIrqW() |
1252 { |
1321 { |
1253 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
1322 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
1254 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,i_GicCpuIfcAddr)); |
1323 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iGicCpuIfcAddr)); |
1255 asm("ldr r2, [r0, #0] "); |
1324 asm("ldr r2, [r0, #0] "); |
1256 asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(GicCpuIfc,iHighestPending)); |
1325 asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(GicCpuIfc,iHighestPending)); |
1257 asm("add r2, r2, #0x00010000 "); /* increment out.w */ |
1326 asm("add r2, r2, #0x00010000 "); /* increment out.w */ |
1258 asm("tst r2, #0x00FF0000 "); /* if wraparound, revert carry */ |
1327 asm("tst r2, #0x00FF0000 "); /* if wraparound, revert carry */ |
1259 asm("subeq r2, r2, #0x01000000 "); |
1328 asm("subeq r2, r2, #0x01000000 "); |
1298 RWSPIN_RLOCK_MARK_ACQ() |
1367 RWSPIN_RLOCK_MARK_ACQ() |
1299 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
1368 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
1300 __JUMP(,lr); |
1369 __JUMP(,lr); |
1301 |
1370 |
1302 asm("2: "); |
1371 asm("2: "); |
1372 asm("ldr r1, __CrashState "); |
|
1303 ARM_WFE; |
1373 ARM_WFE; |
1304 asm("ldr r1, [r0, #0] "); /* read out.w count again */ |
1374 asm("ldr r1, [r1] "); /* check for system crash while we were waiting */ |
1305 asm("b 3b "); |
1375 asm("cmp r1, #0 "); |
1376 asm("bne 9f "); |
|
1377 asm("ldr r1, [r0, #0] "); /* read out.w count again */ |
|
1378 asm("b 3b "); |
|
1379 asm("9: "); |
|
1380 __ASM_CRASH(); /* system crashed while we were waiting */ |
|
1306 } |
1381 } |
1307 |
1382 |
1308 __NAKED__ EXPORT_C void TRWSpinLock::UnlockOnlyR() |
1383 __NAKED__ EXPORT_C void TRWSpinLock::UnlockOnlyR() |
1309 { |
1384 { |
1310 RWSPIN_RUNLOCK_ENTRY_CHECK() |
1385 RWSPIN_RUNLOCK_ENTRY_CHECK() |
1360 RWSPIN_WLOCK_MARK_ACQ() |
1435 RWSPIN_WLOCK_MARK_ACQ() |
1361 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
1436 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
1362 __JUMP(,lr); |
1437 __JUMP(,lr); |
1363 |
1438 |
1364 asm("2: "); |
1439 asm("2: "); |
1440 asm("ldr r1, __CrashState "); |
|
1365 ARM_WFE; |
1441 ARM_WFE; |
1366 asm("ldr r1, [r0, #0] "); /* read out count again */ |
1442 asm("ldr r1, [r1] "); /* check for system crash while we were waiting */ |
1367 asm("b 3b "); |
1443 asm("cmp r1, #0 "); |
1444 asm("bne 9f "); |
|
1445 asm("ldr r1, [r0, #0] "); /* read out count again */ |
|
1446 asm("b 3b "); |
|
1447 asm("9: "); |
|
1448 __ASM_CRASH(); /* system crashed while we were waiting */ |
|
1368 } |
1449 } |
1369 |
1450 |
1370 __NAKED__ EXPORT_C void TRWSpinLock::UnlockOnlyW() |
1451 __NAKED__ EXPORT_C void TRWSpinLock::UnlockOnlyW() |
1371 { |
1452 { |
1372 RWSPIN_WUNLOCK_ENTRY_CHECK() |
1453 RWSPIN_WUNLOCK_ENTRY_CHECK() |
1426 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
1507 __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 */ |
1508 asm("and r0, r12, #%a0" : : "i" ((TInt)KAllInterruptsMask)); /* return original CPSR I and F bits */ |
1428 __JUMP(,lr); |
1509 __JUMP(,lr); |
1429 |
1510 |
1430 asm("2: "); |
1511 asm("2: "); |
1512 asm("ldr r1, __CrashState "); |
|
1431 ARM_WFE; |
1513 ARM_WFE; |
1432 asm("ldr r1, [r0, #0] "); /* read out.w count again */ |
1514 asm("ldr r1, [r1] "); /* check for system crash while we were waiting */ |
1433 asm("b 3b "); |
1515 asm("cmp r1, #0 "); |
1516 asm("bne 9f "); |
|
1517 asm("ldr r1, [r0, #0] "); /* read out.w count again */ |
|
1518 asm("b 3b "); |
|
1519 asm("9: "); |
|
1520 __ASM_CRASH(); /* system crashed while we were waiting */ |
|
1434 } |
1521 } |
1435 |
1522 |
1436 __NAKED__ EXPORT_C void TRWSpinLock::UnlockIrqRestoreR(TInt) |
1523 __NAKED__ EXPORT_C void TRWSpinLock::UnlockIrqRestoreR(TInt) |
1437 { |
1524 { |
1438 RWSPIN_RUNLOCK_ENTRY_CHECK() |
1525 RWSPIN_RUNLOCK_ENTRY_CHECK() |
1453 } |
1540 } |
1454 |
1541 |
1455 __NAKED__ EXPORT_C TBool TRWSpinLock::FlashIrqRestoreR(TInt) |
1542 __NAKED__ EXPORT_C TBool TRWSpinLock::FlashIrqRestoreR(TInt) |
1456 { |
1543 { |
1457 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
1544 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
1458 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,i_GicCpuIfcAddr)); |
1545 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iGicCpuIfcAddr)); |
1459 asm("ldr r2, [r0, #0] "); |
1546 asm("ldr r2, [r0, #0] "); |
1460 asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(GicCpuIfc,iHighestPending)); |
1547 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 */ |
1548 asm("eor r2, r2, r2, lsr #16 "); /* r2 low byte = out.w ^ in.w = 0 if no writers waiting */ |
1462 asm("tst r2, #0xFF "); |
1549 asm("tst r2, #0xFF "); |
1463 asm("addeq r3, r3, #1 "); |
1550 asm("addeq r3, r3, #1 "); |
1500 __DATA_MEMORY_BARRIER__(r3); /* we have got the lock */ |
1587 __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 */ |
1588 asm("and r0, r12, #%a0" : : "i" ((TInt)KAllInterruptsMask)); /* return original CPSR I and F bits */ |
1502 __JUMP(,lr); |
1589 __JUMP(,lr); |
1503 |
1590 |
1504 asm("2: "); |
1591 asm("2: "); |
1592 asm("ldr r1, __CrashState "); |
|
1505 ARM_WFE; |
1593 ARM_WFE; |
1506 asm("ldr r1, [r0, #0] "); /* read out count again */ |
1594 asm("ldr r1, [r1] "); /* check for system crash while we were waiting */ |
1507 asm("b 3b "); |
1595 asm("cmp r1, #0 "); |
1596 asm("bne 9f "); |
|
1597 asm("ldr r1, [r0, #0] "); /* read out count again */ |
|
1598 asm("b 3b "); |
|
1599 asm("9: "); |
|
1600 __ASM_CRASH(); /* system crashed while we were waiting */ |
|
1508 } |
1601 } |
1509 |
1602 |
1510 __NAKED__ EXPORT_C void TRWSpinLock::UnlockIrqRestoreW(TInt) |
1603 __NAKED__ EXPORT_C void TRWSpinLock::UnlockIrqRestoreW(TInt) |
1511 { |
1604 { |
1512 RWSPIN_WUNLOCK_ENTRY_CHECK() |
1605 RWSPIN_WUNLOCK_ENTRY_CHECK() |
1524 } |
1617 } |
1525 |
1618 |
1526 __NAKED__ EXPORT_C TBool TRWSpinLock::FlashIrqRestoreW(TInt) |
1619 __NAKED__ EXPORT_C TBool TRWSpinLock::FlashIrqRestoreW(TInt) |
1527 { |
1620 { |
1528 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
1621 GET_RWNO_TID(,r12); /* r12 -> TSubScheduler */ |
1529 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,i_GicCpuIfcAddr)); |
1622 asm("ldr r12, [r12, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iGicCpuIfcAddr)); |
1530 asm("ldr r2, [r0, #0] "); |
1623 asm("ldr r2, [r0, #0] "); |
1531 asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(GicCpuIfc,iHighestPending)); |
1624 asm("ldr r3, [r12, #%a0]" : : "i" _FOFF(GicCpuIfc,iHighestPending)); |
1532 asm("add r2, r2, #0x00010000 "); /* increment out.w */ |
1625 asm("add r2, r2, #0x00010000 "); /* increment out.w */ |
1533 asm("tst r2, #0x00FF0000 "); /* if wraparound, revert carry */ |
1626 asm("tst r2, #0x00FF0000 "); /* if wraparound, revert carry */ |
1534 asm("subeq r2, r2, #0x01000000 "); |
1627 asm("subeq r2, r2, #0x01000000 "); |