From b7fdbc0ad78c8972c75ad64ce752909871e1ce31 Mon Sep 17 00:00:00 2001
From: "Wladimir J. van der Laan" <laanwj@protonmail.com>
Date: Fri, 7 Feb 2020 18:00:01 +0000
Subject: [PATCH] rust: Add module for timing/FPS measurement

---
 rust/k210-shared/src/lib.rs    |  1 +
 rust/k210-shared/src/timing.rs | 67 ++++++++++++++++++++++++++++++++++
 2 files changed, 68 insertions(+)
 create mode 100644 rust/k210-shared/src/timing.rs

diff --git a/rust/k210-shared/src/lib.rs b/rust/k210-shared/src/lib.rs
index 0077366..f39e0e6 100644
--- a/rust/k210-shared/src/lib.rs
+++ b/rust/k210-shared/src/lib.rs
@@ -7,4 +7,5 @@ pub mod board;
 #[cfg(not(test))]
 pub mod panic;
 pub mod soc;
+pub mod timing;
 mod util;
diff --git a/rust/k210-shared/src/timing.rs b/rust/k210-shared/src/timing.rs
new file mode 100644
index 0000000..0e5512d
--- /dev/null
+++ b/rust/k210-shared/src/timing.rs
@@ -0,0 +1,67 @@
+/** Utilities for measuring time and framerates. */
+use core::cmp;
+use crate::soc::sysctl;
+use riscv::register::mcycle;
+
+/** Number of frame times to store. */
+const N_FRAMES: usize = 100;
+/** Use last N seconds for statistics. */
+const N_SEC: u64 = 2;
+
+/** Counter for FPS statistics.
+ */
+pub struct FPSTimer {
+    /** An array of the times of the last N frames. */
+    frame_times: [u64; N_FRAMES],
+    /** Current offset in ring buffer. */
+    ofs: usize,
+}
+
+impl FPSTimer {
+    pub fn new() -> Self {
+        Self {
+            frame_times: [0; N_FRAMES],
+            ofs: 0,
+        }
+    }
+
+    pub fn frame(&mut self) {
+        self.frame_times[self.ofs] = mcycle::read64();
+        self.ofs += 1;
+        if self.ofs == N_FRAMES {
+            self.ofs = 0;
+        }
+    }
+
+    pub fn fps(&self) -> u32 {
+        let freq = sysctl::clock_get_freq(sysctl::clock::CPU) as u64;
+        let now = mcycle::read64();
+        let oldest = now.saturating_sub(freq * N_SEC);
+        let mut count: u32 = 0;
+        let mut min: u64 = u64::max_value();
+        for &t in self.frame_times.iter() {
+            if t > oldest {
+                count += 1;
+            }
+            if t != 0 {
+                min = cmp::min(min, t);
+            }
+        }
+        if count == 0 {
+            0
+        } else if min <= oldest {
+            // Collected full N seconds
+            count / (N_SEC as u32)
+        } else {
+            // Collected less than N seconds, make an estimate based on what we have
+            (u64::from(count) * freq / (now - min)) as u32
+        }
+    }
+}
+
+/** Return time in microseconds. The starting point is undefined, only differences make sense. */
+pub fn clock() -> u64 {
+    let freq = sysctl::clock_get_freq(sysctl::clock::CPU) as u64;
+    let cycles = mcycle::read64();
+    return cycles * 1_000_000 / freq;
+}