uintset: Add l_uintset_foreach

index 1d96e68..3f20098 100644
@@ -2,7 +2,7 @@
  *
  *  Embedded Linux library
  *
- *  Copyright (C) 2015  Intel Corporation. All rights reserved.
+ *  Copyright (C) 2015-2019  Intel Corporation. All rights reserved.
  *
  *  This library is free software; you can redistribute it and/or
  *  modify it under the terms of the GNU Lesser General Public

@@ -122,6 +122,49 @@ static unsigned long find_last_bit(const unsigned long *addr, unsigned int size)
 	return size;
 }
 
+static unsigned long find_next_bit(const unsigned long *addr,
+							unsigned long size,
+							unsigned long bit)
+{
+	unsigned long mask;
+	unsigned long offset;
+
+	if (bit >= size)
+		return size;
+
+	addr += bit / BITS_PER_LONG;
+	offset = bit % BITS_PER_LONG;
+	bit -= offset;
+
+	if (offset) {
+		mask = *addr & ~(~0UL >> (BITS_PER_LONG - offset));
+		if (mask)
+			return bit + __ffs(mask);
+
+		bit += BITS_PER_LONG;
+		addr++;
+	}
+
+	for (size -= bit; size >= BITS_PER_LONG;
+			size -= BITS_PER_LONG, bit += BITS_PER_LONG, addr++) {
+		if (!*addr)
+			continue;
+
+		return bit + __ffs(*addr);
+	}
+
+	if (!size)
+		return bit;
+
+	mask = *addr & (~0UL >> (BITS_PER_LONG - size));
+	if (mask)
+		bit += __ffs(mask);
+	else
+		bit += size;
+
+	return bit;
+}
+
 struct l_uintset {
 	unsigned long *bits;
 	uint16_t size;

@@ -404,3 +447,25 @@ LIB_EXPORT uint32_t l_uintset_find_min(struct l_uintset *set)
 
 	return bit + set->min;
 }
+
+/**
+ * l_uintset_foreach:
+ * @set: The set of numbers
+ * @function: callback function
+ * @user_data: user data given to callback function
+ *
+ * Call @function for every given number in @set.
+ **/
+LIB_EXPORT void l_uintset_foreach(struct l_uintset *set,
+					l_uintset_foreach_func_t function,
+					void *user_data)
+{
+	unsigned int bit;
+
+	if (unlikely(!set || !function))
+		return;
+
+	for (bit = find_first_bit(set->bits, set->size); bit < set->size;
+			bit = find_next_bit(set->bits, set->size, bit + 1))
+		function(set->min + bit, user_data);
+}

index 474c5e4..dd03cd7 100644
@@ -2,7 +2,7 @@
  *
  *  Embedded Linux library
  *
- *  Copyright (C) 2015  Intel Corporation. All rights reserved.
+ *  Copyright (C) 2015-2019  Intel Corporation. All rights reserved.
  *
  *  This library is free software; you can redistribute it and/or
  *  modify it under the terms of the GNU Lesser General Public

@@ -31,6 +31,8 @@ extern "C" {
 #include <stddef.h>
 #include <stdbool.h>
 
+typedef void (*l_uintset_foreach_func_t) (uint32_t number, void *user_data);
+
 struct l_uintset;
 
 struct l_uintset *l_uintset_new_from_range(uint32_t min, uint32_t max);

@@ -50,6 +52,9 @@ uint32_t l_uintset_find_min(struct l_uintset *set);
 uint32_t l_uintset_find_unused_min(struct l_uintset *set);
 uint32_t l_uintset_find_unused(struct l_uintset *set, uint32_t start);
 
+void l_uintset_foreach(struct l_uintset *set,
+			l_uintset_foreach_func_t function, void *user_data);
+
 #ifdef __cplusplus
 }
 #endif

index 42d9698..4488cd9 100644
@@ -2,7 +2,7 @@
  *
  *  Embedded Linux library
  *
- *  Copyright (C) 2015  Intel Corporation. All rights reserved.
+ *  Copyright (C) 2015-2019  Intel Corporation. All rights reserved.
  *
  *  This library is free software; you can redistribute it and/or
  *  modify it under the terms of the GNU Lesser General Public

@@ -167,6 +167,101 @@ static void test_uintset_find_unused(const void *data)
 	l_uintset_free(set);
 }
 
+static void uintset_foreach(uint32_t number, void *user_data)
+{
+	struct l_uintset *check = user_data;
+
+	l_uintset_take(check, number);
+}
+
+static void test_uintset_foreach(const void *data)
+{
+	struct l_uintset *set;
+	struct l_uintset *check;
+	int i;
+
+	set = l_uintset_new_from_range(0, 63);
+	check = l_uintset_new_from_range(0, 63);
+	assert(set);
+	assert(check);
+
+	for (i = 0; i < 64; i++) {
+		assert(l_uintset_put(set, i));
+		assert(l_uintset_put(check, i));
+	}
+
+	l_uintset_foreach(set, uintset_foreach, check);
+	assert(l_uintset_find_max(check) == 64);
+
+	l_uintset_free(set);
+	l_uintset_free(check);
+
+	set = l_uintset_new_from_range(0, 127);
+	check = l_uintset_new_from_range(0, 127);
+	assert(set);
+	assert(check);
+
+	assert(l_uintset_put(set, 127));
+	assert(l_uintset_put(check, 127));
+
+	l_uintset_foreach(set, uintset_foreach, check);
+	assert(l_uintset_find_max(check) == 128);
+
+	l_uintset_free(set);
+	l_uintset_free(check);
+
+	set = l_uintset_new_from_range(0, 191);
+	check = l_uintset_new_from_range(0, 191);
+	assert(set);
+	assert(check);
+
+	assert(l_uintset_put(set, 50));
+	assert(l_uintset_put(check, 50));
+	assert(l_uintset_put(set, 150));
+	assert(l_uintset_put(check, 150));
+
+	l_uintset_foreach(set, uintset_foreach, check);
+	assert(l_uintset_find_max(check) == 192);
+
+	l_uintset_free(set);
+	l_uintset_free(check);
+
+	set = l_uintset_new_from_range(0, 192);
+	check = l_uintset_new_from_range(0, 192);
+	assert(set);
+	assert(check);
+
+	assert(l_uintset_put(set, 0));
+	assert(l_uintset_put(check, 0));
+	assert(l_uintset_put(set, 63));
+	assert(l_uintset_put(check, 63));
+	assert(l_uintset_put(set, 120));
+	assert(l_uintset_put(check, 120));
+
+	l_uintset_foreach(set, uintset_foreach, check);
+	assert(l_uintset_find_max(check) == 193);
+
+	l_uintset_free(set);
+	l_uintset_free(check);
+
+	set = l_uintset_new_from_range(0, 192);
+	check = l_uintset_new_from_range(0, 192);
+	assert(set);
+	assert(check);
+
+	assert(l_uintset_put(set, 0));
+	assert(l_uintset_put(check, 0));
+
+	assert(l_uintset_put(set, 192));
+	assert(l_uintset_put(check, 192));
+
+	l_uintset_foreach(set, uintset_foreach, check);
+	assert(l_uintset_find_max(check) == 193);
+
+	l_uintset_free(set);
+	l_uintset_free(check);
+}
+
 int main(int argc, char *argv[])
 {
 	l_test_init(&argc, &argv);

@@ -175,6 +270,7 @@ int main(int argc, char *argv[])
 	l_test_add("l_uintset sanity check #2", test_uintset_2, NULL);
 	l_test_add("l_uintset sanity check #3", test_uintset_3, NULL);
 	l_test_add("l_uintset sanity check #4", test_uintset_4, NULL);
+	l_test_add("l_uintset for each tests", test_uintset_foreach, NULL);
 	l_test_add("l_uintset find unused tests", test_uintset_find_unused,
 							NULL);