From e96507f55fd0990e4997275a957808b189bba0eb Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 5 Feb 2022 12:41:09 +1100
Subject: [PATCH] AP_Math: added crc_crc64()

---
 libraries/AP_Math/crc.cpp | 26 ++++++++++++++++++++++++++
 libraries/AP_Math/crc.h   |  2 ++
 2 files changed, 28 insertions(+)

diff --git a/libraries/AP_Math/crc.cpp b/libraries/AP_Math/crc.cpp
index 7b1220415a..9027477166 100644
--- a/libraries/AP_Math/crc.cpp
+++ b/libraries/AP_Math/crc.cpp
@@ -468,3 +468,29 @@ uint16_t crc_crc16_ibm(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_
 
     return crc_accum;
 }
+
+/*
+  64 bit crc matching px4 bootloader
+*/
+uint64_t crc_crc64(const uint32_t *data, uint16_t num_words)
+{
+    const uint64_t poly = 0x42F0E1EBA9EA3693ULL;
+    uint64_t crc = ~(0ULL);
+    while (num_words--) {
+        uint32_t value = *data++;
+        for (uint8_t j = 0; j < 4; j++) {
+            uint8_t byte = ((uint8_t *)&value)[j];
+            crc ^= (uint64_t)byte << 56u;
+            for (uint8_t i = 0; i < 8; i++) {
+                if (crc & (1ull << 63u)) {
+                    crc = (uint64_t)(crc << 1u) ^ poly;
+                } else {
+                    crc = (uint64_t)(crc << 1u);
+                }
+            }
+        }
+    }
+    crc ^= ~(0ULL);
+
+    return crc;
+}
diff --git a/libraries/AP_Math/crc.h b/libraries/AP_Math/crc.h
index 8012676394..dc8daaaf6d 100644
--- a/libraries/AP_Math/crc.h
+++ b/libraries/AP_Math/crc.h
@@ -48,3 +48,5 @@ uint16_t calc_crc_modbus(uint8_t *buf, uint16_t len);
 #define FNV_1_OFFSET_BASIS_64 14695981039346656037UL
 void hash_fnv_1a(uint32_t len, const uint8_t* buf, uint64_t* hash);
 
+// CRC-64-WE using the polynomial of 0x42F0E1EBA9EA3693
+uint64_t crc_crc64(const uint32_t *data, uint16_t num_words);