1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * Copyright (C) 2025 Google LLC
4 * Author: Vincent Donnefort <vdonnefort@google.com>
5 */
6
7 #include <nvhe/clock.h>
8
9 #include <asm/arch_timer.h>
10 #include <asm/div64.h>
11
12 static struct clock_data {
13 struct {
14 u32 mult;
15 u32 shift;
16 u64 epoch_ns;
17 u64 epoch_cyc;
18 u64 cyc_overflow64;
19 } data[2];
20 u64 cur;
21 } trace_clock_data;
22
__clock_mult_uint128(u64 cyc,u32 mult,u32 shift)23 static u64 __clock_mult_uint128(u64 cyc, u32 mult, u32 shift)
24 {
25 __uint128_t ns = (__uint128_t)cyc * mult;
26
27 ns >>= shift;
28
29 return (u64)ns;
30 }
31
32 /* Does not guarantee no reader on the modified bank. */
trace_clock_update(u32 mult,u32 shift,u64 epoch_ns,u64 epoch_cyc)33 void trace_clock_update(u32 mult, u32 shift, u64 epoch_ns, u64 epoch_cyc)
34 {
35 struct clock_data *clock = &trace_clock_data;
36 u64 bank = clock->cur ^ 1;
37
38 if (!mult || shift >= 64)
39 return;
40
41 clock->data[bank].mult = mult;
42 clock->data[bank].shift = shift;
43 clock->data[bank].epoch_ns = epoch_ns;
44 clock->data[bank].epoch_cyc = epoch_cyc;
45 clock->data[bank].cyc_overflow64 = ULONG_MAX / mult;
46
47 smp_store_release(&clock->cur, bank);
48 }
49
50 /* Use untrusted host data */
trace_clock(void)51 u64 trace_clock(void)
52 {
53 struct clock_data *clock = &trace_clock_data;
54 u64 bank = smp_load_acquire(&clock->cur);
55 u64 cyc, ns;
56
57 cyc = __arch_counter_get_cntvct() - clock->data[bank].epoch_cyc;
58
59 if (likely(cyc < clock->data[bank].cyc_overflow64)) {
60 ns = cyc * clock->data[bank].mult;
61 ns >>= clock->data[bank].shift;
62 } else {
63 ns = __clock_mult_uint128(cyc, clock->data[bank].mult,
64 clock->data[bank].shift);
65 }
66
67 return (u64)ns + clock->data[bank].epoch_ns;
68 }
69