From 388fab1ef8e2290eaca09643e528859a2a60c14e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 18 Dec 2023 12:15:52 +1100
Subject: [PATCH] Tools: added testing of _hrt_div1000()

---
 Tools/CPUInfo/CPUInfo.cpp | 43 +++++++++++++++++++++++++++++++++++++--
 1 file changed, 41 insertions(+), 2 deletions(-)

diff --git a/Tools/CPUInfo/CPUInfo.cpp b/Tools/CPUInfo/CPUInfo.cpp
index 4929cc51a7..2b2ee92752 100644
--- a/Tools/CPUInfo/CPUInfo.cpp
+++ b/Tools/CPUInfo/CPUInfo.cpp
@@ -13,10 +13,13 @@
 #include <AP_ESC_Telem/AP_ESC_Telem.h>
 #include "EKF_Maths.h"
 
-#if HAL_WITH_DSP && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+#if HAL_WITH_DSP
 #include <arm_math.h>
-#include <hrt.h>
 #endif
+#include <hrt.h>
+#include <ch.h>
+#endif // HAL_BOARD_CHIBIOS
 
 void setup();
 void loop();
@@ -202,11 +205,47 @@ static void show_timings(void)
     TIMEIT("SEM", { WITH_SEMAPHORE(sem); v_out_32 += v_32;}, 100);
 }
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+static void test_div1000(void)
+{
+    hal.console->printf("Testing div1000\n");
+    for (uint32_t i=0; i<2000000; i++) {
+        uint64_t v;
+        hal.util->get_random_vals((uint8_t*)&v, sizeof(v));
+        uint64_t v1 = v / 1000ULL;
+        uint64_t v2 = _hrt_div1000(v);
+        if (v1 != v2) {
+            AP_HAL::panic("ERROR: 0x%llx v1=0x%llx v2=0x%llx\n",
+                          (unsigned long long)v, (unsigned long long)v1, (unsigned long long)v2);
+            return;
+        }
+    }
+    // test from locked context
+    for (uint32_t i=0; i<2000000; i++) {
+        uint64_t v;
+        hal.util->get_random_vals((uint8_t*)&v, sizeof(v));
+        chSysLock();
+        uint64_t v1 = v / 1000ULL;
+        uint64_t v2 = _hrt_div1000(v);
+        chSysUnlock();
+        if (v1 != v2) {
+            AP_HAL::panic("ERROR: 0x%llx v1=0x%llx v2=0x%llx\n",
+                          (unsigned long long)v, (unsigned long long)v1, (unsigned long long)v2);
+            return;
+        }
+    }
+    hal.console->printf("div1000 OK\n");
+}
+#endif
+
 void loop()
 {
     show_sizes();
     hal.console->printf("\n");
     show_timings();
+#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+    test_div1000();
+#endif
     hal.console->printf("\n");
     hal.scheduler->delay(3000);
 }