Add constant time compare to AES-CCM

Signed-off-by: Aiden Fox Ivey <aiden@aidenfoxivey.com>

src/mesh/aes-ccm.cpp

@@ -10,6 +10,31 @@
 #include "aes-ccm.h"
 #if !MESHTASTIC_EXCLUDE_PKI
 
+/**
+ * Constant-time comparison of two byte arrays
+ *
+ * @param a First byte array to compare
+ * @param b Second byte array to compare
+ * @param len Number of bytes to compare
+ * @return 0 if arrays are equal, 1 if different or if inputs are invalid
+ */
+static int constant_time_compare(const void *a_, const void *b_, size_t len)
+{
+    // Cast to volatile to prevent the compiler from optimizing out their comparison.
+    const volatile uint8_t *volatile a = (const volatile uint8_t *volatile) a_;
+    const volatile uint8_t *volatile b = (const volatile uint8_t *volatile) b_;
+    if (len == 0)
+        return 0;
+    if (a == NULL || b == NULL)
+        return 1;
+    size_t i;
+    volatile uint8_t d = 0U;
+    for (i = 0U; i < len; i++) {
+        d |= (a[i] ^ b[i]);
+    }
+    return (1 & ((d - 1) >> 8)) - 1;
+}
+
 static void WPA_PUT_BE16(uint8_t *a, uint16_t val)
 {
     a[0] = val >> 8;
@@ -146,7 +171,7 @@ bool aes_ccm_ad(const uint8_t *key, size_t key_len, const uint8_t *nonce, size_t
     aes_ccm_encr(L, crypt, crypt_len, plain, a);
     aes_ccm_auth_start(M, L, nonce, aad, aad_len, crypt_len, x);
     aes_ccm_auth(plain, crypt_len, x);
-    if (memcmp(x, t, M) != 0) { // FIXME make const comp
+    if (constant_time_compare(x, t, M) != 0) {
         return false;
     }
     return true;