Make clock.c atomic

core/clock.c

@@ -1,54 +1,50 @@
 #define pr_fmt(fmt) "clock: " fmt
 
+#include <zinnia/atomic.h>
 #include <zinnia/clock.h>
 #include <zinnia/compiler.h>
 #include <zinnia/print.h>
-#include <zinnia/spin.h>
 #include <arch/cpu.h>
 #include <stdint.h>
 
-static struct clock* active_clock = nullptr;
-static uint64_t counter_base = 0;
-static struct spinlock lock = {};
+static _Atomic(struct clock*) active_clock;
+static _Atomic(int64_t) realtime_offset_ns;
 
-static uint64_t clock_get_elapsed_ns_locked() {
-    if (__unlikely(!active_clock))
+static uint64_t clock_read_elapsed(struct clock* clock) {
+    if (__unlikely(!clock))
         return 0;
 
-    return counter_base + active_clock->get_elapsed_ns(active_clock);
+    return clock->base + clock->get_elapsed_ns(clock);
 }
 
 bool clock_available() {
-    spin_lock(&lock);
-    bool available = active_clock != nullptr;
-    spin_unlock(&lock);
-    return available;
+    return atomic_load(&active_clock, ATOMIC_ACQUIRE) != nullptr;
 }
 
 uint64_t clock_get_elapsed_ns() {
-    spin_lock(&lock);
-    uint64_t elapsed = clock_get_elapsed_ns_locked();
-    spin_unlock(&lock);
-    return elapsed;
+    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) {
-    spin_lock(&lock);
+    struct clock* old = atomic_load(&active_clock, ATOMIC_ACQUIRE);
 
-    if (active_clock) {
-        if (clock->priority <= active_clock->priority) {
-            spin_unlock(&lock);
+    do {
+        if (old && clock->priority <= old->priority)
             return false;
-        }
-    }
 
-    uint64_t elapsed = clock_get_elapsed_ns_locked();
+        uint64_t elapsed = clock_read_elapsed(old);
         clock->reset(clock);
-
-    counter_base = elapsed;
-    active_clock = clock;
-
-    spin_unlock(&lock);
+        clock->base = elapsed;
+    } while (!atomic_compare_exchange(&active_clock, &old, clock, ATOMIC_RELEASE, ATOMIC_ACQUIRE));
 
     log("Switching to clock source \"%s\"\n", clock->name);
 

include/zinnia/clock.h

@@ -5,6 +5,7 @@
 struct clock {
     const char* name;
     uint8_t priority;
+    uint64_t base;
 
     void (*reset)(struct clock* c);
     uint64_t (*get_elapsed_ns)(struct clock* c);
@@ -19,5 +20,9 @@ bool clock_available();
 // Gets the current timestamp in nanoseconds since the last reset.
 uint64_t clock_get_elapsed_ns();
 
+uint64_t clock_get_realtime_ns();
+
+void clock_set_realtime_ns(uint64_t ns);
+
 // Spin for `ns` nanoseconds.
 void clock_spin_ns(uint64_t ns);