From 4dd0594d09a2eb081403c49befbe7a0db4184442 Mon Sep 17 00:00:00 2001
From: Benjamin Valentin <benjamin.valentin@ml-pa.com>
Date: Thu, 10 Nov 2022 23:40:03 +0100
Subject: [PATCH] sys/bitfield: add bf_popcnt()

---
 sys/bitfield/bitfield.c | 19 +++++++++++++++++++
 sys/include/bitfield.h  | 10 ++++++++++
 2 files changed, 29 insertions(+)

diff --git a/sys/bitfield/bitfield.c b/sys/bitfield/bitfield.c
index d6482e9893..bb33165805 100644
--- a/sys/bitfield/bitfield.c
+++ b/sys/bitfield/bitfield.c
@@ -21,6 +21,7 @@
 #include <stdint.h>
 #include <string.h>
 #include "bitfield.h"
+#include "bitarithm.h"
 #include "irq.h"
 
 static inline unsigned _skip_bytes(const uint8_t field[], unsigned nbytes, uint8_t byte)
@@ -92,3 +93,21 @@ void bf_set_all(uint8_t field[], size_t size)
         field[bytes] = ~((1U << (8 - bits)) - 1);
     }
 }
+
+unsigned bf_popcnt(const uint8_t field[], size_t size)
+{
+    unsigned bytes = size >> 3;
+    unsigned bits = size & 0x7;
+    unsigned mask = ~((1U << (8 - bits)) - 1);
+
+    unsigned count = 0;
+    for (unsigned i = 0; i < bytes; ++i) {
+        count += bitarithm_bits_set(field[i]);
+    }
+
+    if (bits) {
+        count += bitarithm_bits_set(field[bytes] & mask);
+    }
+
+    return count;
+}
diff --git a/sys/include/bitfield.h b/sys/include/bitfield.h
index f8f1a9b0d0..79c842db95 100644
--- a/sys/include/bitfield.h
+++ b/sys/include/bitfield.h
@@ -219,6 +219,16 @@ int bf_find_first_unset(const uint8_t field[], size_t size);
  */
 void bf_set_all(uint8_t field[], size_t size);
 
+/**
+ * @brief  Count set bits in the bitfield
+ *
+ * @param[in]     field The bitfield
+ * @param[in]     size  The size of the bitfield
+ *
+ * @return number of '1' bits in the bitfield
+ */
+unsigned bf_popcnt(const uint8_t field[], size_t size);
+
 #ifdef __cplusplus
 }
 #endif