302 EXPORT_C __NAKED__ TUint64 NKern::Timestamp() |
299 EXPORT_C __NAKED__ TUint64 NKern::Timestamp() |
303 { |
300 { |
304 asm("ldr r3, __TheScheduler "); |
301 asm("ldr r3, __TheScheduler "); |
305 asm("mrs r12, cpsr "); // r12 = saved interrupt mask |
302 asm("mrs r12, cpsr "); // r12 = saved interrupt mask |
306 asm("stmfd sp!, {r4-r7} "); |
303 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 |
304 asm("ldr r4, [r3, #%a0]" : : "i" _FOFF(TScheduler,iSX.iGlobalTimerAddr)); // r4 points to global timer |
309 __ASM_CLI(); // disable all interrupts |
305 __ASM_CLI(); // disable all interrupts |
310 asm("ldr r6, [r5, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iTicksSinceLastSync)); // r6 = count value of last frequency change (low) |
306 asm("ldr r6, [r3, #%a0]" : : "i" _FOFF(TScheduler,iSX.iCount0)); // r6 = count value of last frequency change (low) |
311 asm("ldr r7, [r5, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iLastTimerSet)); // r7 = count value of last frequency change (high) |
307 asm("ldr r7, [r3, #%a0]" : : "i" (_FOFF(TScheduler,iSX.iCount0)+4)); // r7 = count value of last frequency change (high) |
312 asm("ldr r2, [r4, #%a0]" : : "i" _FOFF(ArmGlobalTimer,iTimerCountHigh)); // r2 = current timer counter high word |
308 asm("ldr r2, [r4, #%a0]" : : "i" _FOFF(ArmGlobalTimer,iTimerCountHigh)); // r2 = current timer counter high word |
313 |
309 |
314 // To read 64 bit timer value, read high, low, high |
310 // To read 64 bit timer value, read high, low, high |
315 // If two high values match -> OK, else repeat |
311 // If two high values match -> OK, else repeat |
316 asm("1: "); |
312 asm("1: "); |
317 asm("mov r1, r2 "); // r1 = previous value of timer counter high word |
313 asm("mov r1, r2 "); // r1 = previous value of timer counter high word |
318 asm("ldr r0, [r4, #%a0]" : : "i" _FOFF(ArmGlobalTimer,iTimerCountLow)); // r0 = current timer counter low word |
314 asm("ldr r0, [r4, #%a0]" : : "i" _FOFF(ArmGlobalTimer,iTimerCountLow)); // r0 = current timer counter low word |
319 asm("ldr r2, [r4, #%a0]" : : "i" _FOFF(ArmGlobalTimer,iTimerCountHigh)); // r2 = current timer counter high word |
315 asm("ldr r2, [r4, #%a0]" : : "i" _FOFF(ArmGlobalTimer,iTimerCountHigh)); // r2 = current timer counter high word |
|
316 asm("mov r5, r3 "); // r5 = &TheScheduler |
320 asm("cmp r1, r2 "); // high word changed? |
317 asm("cmp r1, r2 "); // high word changed? |
321 asm("bne 1b "); // if so, retry |
318 asm("bne 1b "); // if so, retry |
322 |
319 |
323 // Now have R1:R0 = 64 bit global timer count |
320 // Now have R1:R0 = 64 bit global timer count |
324 asm("ldr r3, [r5, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iNTimerPeriodM)); // r3 = period multiplier |
321 asm("ldr r3, [r5, #%a0]" : : "i" _FOFF(TScheduler,iSX.iGTimerFreqRI.iI.iM)); // r3 = period multiplier |
325 asm("ldr r4, [r5, #%a0]" : : "i" _FOFF(TSubScheduler,iSSX.iNTimerPeriodS)); // r4 = period multiplier shift |
322 asm("ldrsh r4, [r5, #%a0]" : : "i" _FOFF(TScheduler,iSX.iGTimerFreqRI.iI.iX)); // r4 = period multiplier shift |
326 asm("subs r6, r0, r6 "); // r7:r6 = ticks from last frequency change |
323 asm("subs r6, r0, r6 "); // r7:r6 = ticks from last frequency change |
327 asm("sbcs r7, r1, r7 "); |
324 asm("sbcs r7, r1, r7 "); |
328 asm("umull r0, r1, r6, r3 "); |
325 asm("umull r0, r1, r6, r3 "); |
329 asm("mov r2, #0 "); |
326 asm("mov r2, #0 "); |
330 asm("umlal r1, r2, r7, r3 "); // r2:r1:r0 = delta * period multiplier |
327 asm("umlal r1, r2, r7, r3 "); // r2:r1:r0 = delta * period multiplier |
331 asm("rsb r3, r4, #32 "); |
328 asm("ldr r6, [r5, #%a0]!" : : "i" _FOFF(TScheduler,iSX.iTimestamp0)); // r6 = timestamp at last freq change (low) |
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) |
329 asm("ldr r7, [r5, #4] "); // r7 = timestamp at last freq change (high) |
334 asm("msr cpsr, r12 "); // restore interrupts |
330 asm("msr cpsr, r12 "); // restore interrupts |
|
331 asm("rsb r4, r4, #0 "); |
|
332 asm("rsb r3, r4, #32 "); |
335 asm("movs r0, r0, lsr r4 "); // rounding bit into C |
333 asm("movs r0, r0, lsr r4 "); // rounding bit into C |
336 asm("orr r0, r0, r1, lsl r3 "); |
334 asm("orr r0, r0, r1, lsl r3 "); |
337 asm("mov r1, r1, lsr r4 "); |
335 asm("mov r1, r1, lsr r4 "); |
338 asm("orr r1, r1, r2, lsl r3 "); // r1:r0 = (delta * period multiplier) >> period multiplier shift |
336 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 |
337 asm("adcs r0, r0, r6 "); // scaled delta + timestamp at last freq change |
341 asm("ldmfd sp!, {r4-r7} "); |
339 asm("ldmfd sp!, {r4-r7} "); |
342 __JUMP(,lr); |
340 __JUMP(,lr); |
343 |
341 |
344 asm("__TheScheduler: "); |
342 asm("__TheScheduler: "); |
345 asm(".word %a0" : : "i" ((TInt)&TheScheduler)); |
343 asm(".word %a0" : : "i" ((TInt)&TheScheduler)); |
|
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); |
346 } |
396 } |
347 |
397 |
348 #elif defined(__NKERN_TIMESTAMP_USE_INLINE_BSP_CODE__) |
398 #elif defined(__NKERN_TIMESTAMP_USE_INLINE_BSP_CODE__) |
349 #define __DEFINE_NKERN_TIMESTAMP_ASM__ |
399 #define __DEFINE_NKERN_TIMESTAMP_ASM__ |
350 #include <variant_timestamp.h> |
400 #include <variant_timestamp.h> |