From d05de070db4bdee9cb1ffb516da08234d3f6cb68 Mon Sep 17 00:00:00 2001
From: Emil Gydesen <emil.gydesen@nordicsemi.no>
Date: Wed, 15 Jan 2025 14:01:32 +0100
Subject: [PATCH] include: util: Add generic function to count bits set in a
 value
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Adds a generic function that will count the number of bits set in
a value.

It uses POPCOUNT (e.g. __builtin_popcount for GCC) if available,
or else it will use Brian Kernighan’s Algorithm to count bits.

POPCOUNT will likely always support unsigned ints, but the function
was implemented to use it with uint8_t for the sake of simplicity
and compatibility with Brian Kernighan’s Algorithm.

A generic solution was chosen rather than a macro/function per
type (e.g. uint8_t, uint16_t, etc.) as that is easier to maintain
and also supports array types (e.g. counting the number of bits
in 128 or 256 octet arrays).

Signed-off-by: Emil Gydesen <emil.gydesen@nordicsemi.no>
---
 doc/releases/release-notes-4.2.rst |  4 ++++
 include/zephyr/sys/util.h          | 28 ++++++++++++++++++++++++++++
 tests/unit/util/main.c             | 28 ++++++++++++++++++++++++++--
 3 files changed, 58 insertions(+), 2 deletions(-)

diff --git a/doc/releases/release-notes-4.2.rst b/doc/releases/release-notes-4.2.rst
index 51f21cdff69..e08edefb881 100644
--- a/doc/releases/release-notes-4.2.rst
+++ b/doc/releases/release-notes-4.2.rst
@@ -102,6 +102,10 @@ New APIs and options
 
     * LE Connection Subrating is no longer experimental.
 
+* Other
+
+  * :c:func:`count_bits`
+
 New Boards
 **********
 
diff --git a/include/zephyr/sys/util.h b/include/zephyr/sys/util.h
index 0e38acb4aae..338b57e7423 100644
--- a/include/zephyr/sys/util.h
+++ b/include/zephyr/sys/util.h
@@ -783,6 +783,34 @@ static inline void mem_xor_128(uint8_t dst[16], const uint8_t src1[16], const ui
 	mem_xor_n(dst, src1, src2, 16);
 }
 
+/**
+ * @brief Returns the number of bits set in a value
+ *
+ * @param value The value to count number of bits set of
+ * @param len The number of octets in @p value
+ */
+static inline size_t count_bits(const void *value, size_t len)
+{
+	const uint8_t *value_u8 = (const uint8_t *)value;
+	size_t cnt = 0U;
+
+	for (size_t i = 0U; i < len; i++) {
+		if (value_u8[i] != 0U) {
+#ifdef POPCOUNT
+			cnt += POPCOUNT(value_u8[i]);
+#else
+			/* Implements Brian Kernighan’s Algorithm to count bits */
+			while (value_u8[i]) {
+				cnt += value_u8[i] & 1;
+				value_u8[i] >>= 1;
+			}
+#endif
+		}
+	}
+
+	return cnt;
+}
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/tests/unit/util/main.c b/tests/unit/util/main.c
index e01c7dce286..bc5d01aeff7 100644
--- a/tests/unit/util/main.c
+++ b/tests/unit/util/main.c
@@ -1,14 +1,19 @@
 /*
  * Copyright (c) 2019 Oticon A/S
+ * Copyright (c) 2025 Nordic Semiconductor ASA
  *
  * SPDX-License-Identifier: Apache-2.0
  */
 
-#include <zephyr/ztest.h>
-#include <zephyr/sys/util.h>
+#include <stdint.h>
 #include <stdio.h>
 #include <string.h>
 
+#include <zephyr/ztest.h>
+#include <zephyr/sys/util.h>
+#include <zephyr/ztest_assert.h>
+#include <zephyr/ztest_test.h>
+
 ZTEST(util, test_u8_to_dec) {
 	char text[4];
 	uint8_t len;
@@ -768,6 +773,25 @@ ZTEST(util, test_mem_xor_128)
 	zassert_mem_equal(expected_result, dst, 16);
 }
 
+ZTEST(util, test_count_bits)
+{
+	uint8_t zero = 0U;
+	uint8_t u8 = 29U;
+	uint16_t u16 = 29999U;
+	uint32_t u32 = 2999999999U;
+	uint64_t u64 = 123456789012345ULL;
+	uint8_t u8_arr[] = {u8, u8, u8, u8, u8, u8, u8, u8, u8, u8, u8, u8, u8, u8, u8, u8,
+			    u8, u8, u8, u8, u8, u8, u8, u8, u8, u8, u8, u8, u8, u8, u8, u8};
+
+	zassert_equal(count_bits(&zero, sizeof(zero)), 0);
+	zassert_equal(count_bits(&u8, sizeof(u8)), 4);
+	zassert_equal(count_bits(&u16, sizeof(u16)), 10);
+	zassert_equal(count_bits(&u32, sizeof(u32)), 20);
+	zassert_equal(count_bits(&u64, sizeof(u64)), 23);
+
+	zassert_equal(count_bits(u8_arr, sizeof(u8_arr)), 128);
+}
+
 ZTEST(util, test_CONCAT)
 {
 #define _CAT_PART1 1