CircularIndex: Fix non-power-of-two sizes

Add back the option to use CircularIndex even for non-power-of-two sizes
for the future.

src/hal/circular_buffer.h

@@ -12,6 +12,8 @@
 template <typename index_t = uint_fast8_t, index_t size = 16>
 class CircularIndex {
 public:
+    static constexpr bool size_is_power2 = !(size & (size - 1));
+
     static_assert(size <= std::numeric_limits<index_t>::max() / 2,
         "index_t is too small for the requested size");
 
@@ -62,7 +64,17 @@ public:
 
     /// @returns number of elements in the buffer
     inline index_t count() const {
-        return head - tail;
+        if constexpr (size_is_power2)
+            return head - tail;
+        else {
+            index_t i = tail;
+            index_t c = 0;
+            while (i != head) {
+                i = next(i);
+                ++c;
+            }
+            return c;
+        }
     }
 
 protected:
@@ -76,8 +88,10 @@ protected:
     static index_t next(index_t cursor) {
         // note: the modulo can be avoided if size is a power of two: we can do this
         // relying on the optimizer eliding the following check at compile time.
-        static constexpr bool power2 = !(size & (size - 1));
+        if constexpr (size_is_power2)
-        return power2 ? (cursor + 1) : (cursor + 1) % (size * 2);
+            return (cursor + 1);
+        else
+            return (cursor + 1) % (size * 2);
     }
 };