From 49c3536ca7ab2686788702aaee5e526e079337d3 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 20 Dec 2023 14:26:12 +1100
Subject: [PATCH] AP_Math: added uint64_div1000()

and test suite
---
 libraries/AP_Math/div1000.h           | 26 ++++++++++++++++++++++++++
 libraries/AP_Math/tests/test_math.cpp | 12 ++++++++++++
 2 files changed, 38 insertions(+)
 create mode 100644 libraries/AP_Math/div1000.h

diff --git a/libraries/AP_Math/div1000.h b/libraries/AP_Math/div1000.h
new file mode 100644
index 0000000000..e80a52d542
--- /dev/null
+++ b/libraries/AP_Math/div1000.h
@@ -0,0 +1,26 @@
+/*
+  return 64 bit x / 1000
+  faster than the normal gcc implementation using by about 3x
+  With thanks to https://0x414b.com/2021/04/16/arm-division.html
+  and https://stackoverflow.com/questions/74765410/multiply-two-uint64-ts-and-store-result-to-uint64-t-doesnt-seem-to-work
+*/
+static inline uint64_t uint64_div1000(uint64_t x)
+{
+    x >>= 3U;
+    uint64_t a_lo = (uint32_t)x;
+    uint64_t a_hi = x >> 32;
+    const uint64_t b_lo = 0xe353f7cfU;
+    const uint64_t b_hi = 0x20c49ba5U;
+
+    uint64_t a_x_b_hi = a_hi * b_hi;
+    uint64_t a_x_b_mid = a_hi * b_lo;
+    uint64_t b_x_a_mid = b_hi * a_lo;
+    uint32_t a_x_b_lo = (a_lo * b_lo)>>32;
+
+    // 64-bit product + two 32-bit values
+    uint64_t middle = a_x_b_mid + a_x_b_lo + (uint32_t)b_x_a_mid;
+
+    // 64-bit product + two 32-bit values
+    uint64_t r = a_x_b_hi + (middle >> 32) + (b_x_a_mid >> 32);
+    return r >> 4U;
+}
diff --git a/libraries/AP_Math/tests/test_math.cpp b/libraries/AP_Math/tests/test_math.cpp
index 0043a38c5b..afd8a164c8 100644
--- a/libraries/AP_Math/tests/test_math.cpp
+++ b/libraries/AP_Math/tests/test_math.cpp
@@ -7,6 +7,7 @@
 #include <AP_gtest.h>
 
 #include <AP_Math/AP_Math.h>
+#include <AP_Math/div1000.h>
 
 const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 
@@ -692,6 +693,17 @@ TEST(CRCTest, parity)
     EXPECT_EQ(parity(0b11111111), 0);
 }
 
+TEST(MathTest, div1000)
+{
+    for (uint32_t i=0; i<1000000; i++) {
+        uint64_t v;
+        EXPECT_EQ(hal.util->get_random_vals((uint8_t*)&v, sizeof(v)), true);
+        uint64_t v1 = v / 1000ULL;
+        uint64_t v2 = uint64_div1000(v);
+        EXPECT_EQ(v1, v2);
+    }
+}
+
 AP_GTEST_PANIC()
 AP_GTEST_MAIN()