65 lines
1.7 KiB
C
65 lines
1.7 KiB
C
#define pr_fmt(fmt) "clock: " fmt
|
|
|
|
#include <zinnia/atomic.h>
|
|
#include <zinnia/clock.h>
|
|
#include <zinnia/compiler.h>
|
|
#include <zinnia/print.h>
|
|
#include <arch/cpu.h>
|
|
#include <stdint.h>
|
|
|
|
static _Atomic(struct clock*) active_clock;
|
|
static _Atomic(int64_t) realtime_offset_ns;
|
|
|
|
static uint64_t clock_read_elapsed(struct clock* clock) {
|
|
if (__unlikely(!clock))
|
|
return 0;
|
|
|
|
return clock->base + clock->get_elapsed_ns(clock);
|
|
}
|
|
|
|
bool clock_available() {
|
|
return atomic_load(&active_clock, ATOMIC_ACQUIRE) != nullptr;
|
|
}
|
|
|
|
uint64_t clock_get_elapsed_ns() {
|
|
return clock_read_elapsed(atomic_load(&active_clock, ATOMIC_ACQUIRE));
|
|
}
|
|
|
|
uint64_t clock_get_realtime_ns() {
|
|
int64_t ns = (int64_t)clock_get_elapsed_ns() + atomic_load(&realtime_offset_ns, ATOMIC_RELAXED);
|
|
return ns < 0 ? 0 : ns;
|
|
}
|
|
|
|
void clock_set_realtime_ns(uint64_t ns) {
|
|
atomic_store(&realtime_offset_ns, (int64_t)ns - (int64_t)clock_get_elapsed_ns(), ATOMIC_RELAXED);
|
|
}
|
|
|
|
bool clock_switch(struct clock* clock) {
|
|
struct clock* old = atomic_load(&active_clock, ATOMIC_ACQUIRE);
|
|
|
|
do {
|
|
if (old && clock->priority <= old->priority)
|
|
return false;
|
|
|
|
uint64_t elapsed = clock_read_elapsed(old);
|
|
clock->reset(clock);
|
|
clock->base = elapsed;
|
|
} while (!atomic_compare_exchange(&active_clock, &old, clock, ATOMIC_RELEASE, ATOMIC_ACQUIRE));
|
|
|
|
log("Switching to clock source \"%s\"\n", clock->name);
|
|
|
|
return true;
|
|
}
|
|
|
|
void clock_spin_ns(uint64_t ns) {
|
|
if (!clock_available()) {
|
|
log_err("Unable to sleep for %lu ns, no clock available!\n", ns);
|
|
return;
|
|
}
|
|
|
|
uint64_t target = clock_get_elapsed_ns() + ns;
|
|
while (clock_get_elapsed_ns() < target) {
|
|
arch_cpu_pause();
|
|
}
|
|
}
|