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 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. */ 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 */ 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